diff --git a/CMakeLists.txt b/CMakeLists.txt index 5ad2569a7e..ec80adb253 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -626,6 +626,7 @@ VP_OPTION(USE_DIRECTSHOW DIRECTSHOW "" "Include dshow support" " VP_OPTION(USE_OPENMP OpenMP "" "Include openmp support" "" ON) VP_OPTION(USE_EIGEN3 Eigen3 QUIET "Include eigen3 support" "" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_COIN3D "Coin3D;MyCoin3D" "" "Include coin3d support" "" ON IF OPENGL_FOUND AND NOT WINRT AND NOT IOS) +VP_OPTION(USE_PANDA3D "MyPanda3D" "" "Include Panda3D support" "" ON IF NOT IOS) VP_OPTION(USE_YARP YARP QUIET "Include yarp support" "YARP_DIR" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_OGRE OGRE QUIET "Include Ogre support" "OGRE_DIR" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_OIS OIS QUIET "Include Ogre/ois support" "OIS_DIR" ON IF USE_OGRE AND NOT WINRT AND NOT IOS) @@ -1089,6 +1090,7 @@ VP_SET(VISP_HAVE_SOWIN TRUE IF (BUILD_MODULE_visp_ar AND USE_SOWIN)) VP_SET(VISP_HAVE_SOXT TRUE IF (BUILD_MODULE_visp_ar AND USE_SOXT)) VP_SET(VISP_HAVE_SOQT TRUE IF (BUILD_MODULE_visp_ar AND USE_SOQT)) VP_SET(VISP_HAVE_QT TRUE IF (BUILD_MODULE_visp_ar AND USE_QT)) +VP_SET(VISP_HAVE_PANDA3D TRUE IF (BUILD_MODULE_visp_ar AND USE_PANDA3D)) VP_SET(VISP_HAVE_ZBAR TRUE IF (BUILD_MODULE_visp_detection AND USE_ZBAR)) VP_SET(VISP_HAVE_DMTX TRUE IF (BUILD_MODULE_visp_detection AND USE_DMTX)) @@ -1729,13 +1731,15 @@ status(" \\- Use Coin3D:" USE_COIN3D THEN "yes (ver ${COIN3D_VE status(" \\- Use SoWin:" USE_SOWIN THEN "yes" ELSE "no") status(" \\- Use SoXt:" USE_SOXT THEN "yes" ELSE "no") if(USE_SOQT AND SoQt_FOUND) -status(" \\- Use SoQt:" USE_SOQT AND SoQt_FOUND THEN "yes (ver ${SoQt_VERSION})" ELSE "no") +status(" \\- Use SoQt:" USE_SOQT AND SoQt_FOUND THEN "yes (ver ${SoQt_VERSION})" ELSE "no") else() status(" \\- Use SoQt:" USE_SOQT THEN "yes" ELSE "no") endif() status(" \\- Use Qt5:" USE_SOQT AND SoQt_FOUND THEN "yes" ELSE "no") status(" \\- Use Qt4:" USE_QT AND DESIRED_QT_VERSION MATCHES 4 THEN "yes" ELSE "no") status(" \\- Use Qt3:" USE_QT AND DESIRED_QT_VERSION MATCHES 3 THEN "yes" ELSE "no") +status(" Panda3D:" USE_PANDA3D THEN "yes (ver ${Panda3D_VERSION_STRING})" ELSE "no") + status("") status(" Media I/O: ") status(" Use JPEG:" USE_JPEG THEN "yes (ver ${JPEG_LIB_VERSION})" ELSE "no") diff --git a/ChangeLog.txt b/ChangeLog.txt index 8b069dd5c4..df585eecb7 100644 --- a/ChangeLog.txt +++ b/ChangeLog.txt @@ -37,6 +37,8 @@ ViSP 3.x.x (Version in development) . New vpImageTools::inRange() functions to ease binary mask computation by thresholding HSV channels . New tutorials in tutorial/segmentation/color folder to show how to use HSV color segmentation to extract the corresponding point cloud + . New scene renderer based on Panda3D. See inheritance diagram for vpPanda3DBaseRenderer class and corresponding + tutorial. - Applications . Migrate eye-to-hand tutorials in apps - Tutorials @@ -57,6 +59,8 @@ ViSP 3.x.x (Version in development) https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-hsv-segmentation-live.html . New tutorial: Point cloud segmentation using HSV color scale https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-hsv-segmentation-pcl.html + . New tutorial: Rendering a 3D scene with Panda3D + https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-panda3d.html - Bug fixed . [#1251] Bug in vpDisplay::displayFrame() . [#1270] Build issue around std::clamp and optional header which are not found with cxx17 diff --git a/cmake/FindMyPanda3D.cmake b/cmake/FindMyPanda3D.cmake new file mode 100644 index 0000000000..22bbde66c8 --- /dev/null +++ b/cmake/FindMyPanda3D.cmake @@ -0,0 +1,96 @@ +############################################################################# +# +# 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: +# Try to find OpenCV framework. +# +# Panda3D_FOUND +# Panda3D_INCLUDE_DIRS +# Panda3D_LIBRARIES +# +############################################################################# + +set(PANDA3D_INCLUDE_SEARCH_PATHS + $ENV{Panda3D_DIR}/include + $ENV{Panda3D_DIR}/built/include + ${Panda3D_DIR}/include + ${Panda3D_DIR}/built/include + /usr/include/panda3d + /usr/local/include/panda3d + /Library/Developer/Panda3D/include +) + +set(PANDA3D_LIBRARIES_SEARCH_PATHS + $ENV{Panda3D_DIR}/lib + $ENV{Panda3D_DIR}/built/lib + $ENV{Panda3D_DIR}/bin + ${Panda3D_DIR}/lib + ${Panda3D_DIR}/built/lib + ${Panda3D_DIR}/bin + /usr/lib/panda3d + /usr/local/lib/panda3d + /usr/lib/x86_64-linux-gnu/panda3d + /Library/Developer/Panda3D/lib +) + +set(PANDA3D_LIBS + panda p3framework pandaexpress + p3dtoolconfig p3dtool p3direct + #pandaegg + #p3ffmpeg p3interrogatedb p3tinydisplay p3vision + #pandaai pandafx pandaphysics pandaskel +) + +# Fetch all libraries +set(Panda3D_LIBRARIES "") +set(ALL_LIBS_FOUND TRUE) +foreach(lib_name ${PANDA3D_LIBS}) + find_library(Panda3D_${lib_name}_LIBRARY NAMES "lib${lib_name}" ${lib_name} PATHS ${PANDA3D_LIBRARIES_SEARCH_PATHS}) + if(NOT Panda3D_${lib_name}_LIBRARY) + set(ALL_LIBS_FOUND FALSE) + else() + list(APPEND Panda3D_LIBRARIES ${Panda3D_${lib_name}_LIBRARY}) + endif() + + mark_as_advanced(Panda3D_${lib_name}_LIBRARY) +endforeach() + +find_path(Panda3D_INCLUDE_DIRS panda.h PATHS ${PANDA3D_INCLUDE_SEARCH_PATHS}) +message(${Panda3D_INCLUDE_DIRS}) +include(FindPackageHandleStandardArgs) +# Handle the QUIETLY and REQUIRED arguments and set the Panda3D_FOUND to TRUE +# if all listed variables are TRUE +find_package_handle_standard_args(Panda3D DEFAULT_MSG ALL_LIBS_FOUND Panda3D_INCLUDE_DIRS) + +if(Panda3D_FOUND) + vp_parse_header2(Panda3D "${Panda3D_INCLUDE_DIRS}/pandaVersion.h" PANDA_VERSION_STR) +endif() + +mark_as_advanced(Panda3D_INCLUDE_DIRS) diff --git a/cmake/templates/VISPConfig.cmake.in b/cmake/templates/VISPConfig.cmake.in index e84df6cabf..afceccebe1 100644 --- a/cmake/templates/VISPConfig.cmake.in +++ b/cmake/templates/VISPConfig.cmake.in @@ -242,6 +242,7 @@ set(VISP_HAVE_OPENCV_NONFREE "@VISP_HAVE_OPENCV_NONFREE@") set(VISP_HAVE_OPENCV_VERSION "@VISP_HAVE_OPENCV_VERSION@") set(VISP_HAVE_OPENGL "@VISP_HAVE_OPENGL@") set(VISP_HAVE_OPENMP "@VISP_HAVE_OPENMP@") +set(VISP_HAVE_PANDA3D "@VISP_HAVE_PANDA3D@") set(VISP_HAVE_PARPORT "@VISP_HAVE_PARPORT@") set(VISP_HAVE_PCL "@VISP_HAVE_PCL@") set(VISP_HAVE_PIONEER "@VISP_HAVE_PIONEER@") diff --git a/cmake/templates/vpConfig.h.in b/cmake/templates/vpConfig.h.in index 7e50a6141e..13702fed5a 100644 --- a/cmake/templates/vpConfig.h.in +++ b/cmake/templates/vpConfig.h.in @@ -304,6 +304,9 @@ namespace vp = VISP_NAMESPACE_NAME; // Defined if OpenGL library available. #cmakedefine VISP_HAVE_OPENGL +// Defined if Panda3D is available +#cmakedefine VISP_HAVE_PANDA3D + // Defined if Qt library available (either Qt-3 or Qt-4). #cmakedefine VISP_HAVE_QT diff --git a/doc/config-doxygen.in b/doc/config-doxygen.in index 23d5cbff80..e946a03f85 100644 --- a/doc/config-doxygen.in +++ b/doc/config-doxygen.in @@ -2411,6 +2411,7 @@ PREDEFINED = @DOXYGEN_SHOULD_SKIP_THIS@ \ HAVE_OPENCV_VIDEO \ HAVE_OPENCV_VIDEOIO \ VISP_HAVE_OPENGL \ + VISP_HAVE_PANDA3D \ VISP_HAVE_PARPORT \ VISP_HAVE_PCL \ VISP_HAVE_PIONEER \ diff --git a/doc/image/tutorial/renderer/img-cube-rendering-panda3d.png b/doc/image/tutorial/renderer/img-cube-rendering-panda3d.png new file mode 100644 index 0000000000..d2f5881e1a Binary files /dev/null and b/doc/image/tutorial/renderer/img-cube-rendering-panda3d.png differ diff --git a/doc/mainpage.dox.in b/doc/mainpage.dox.in index 20d5c120d3..87ecb769f8 100644 --- a/doc/mainpage.dox.in +++ b/doc/mainpage.dox.in @@ -416,6 +416,27 @@ in different ways. This will motivate us to continue the efforts. \defgroup group_ar_renderer Renderer Renderer interfaces. */ +/*! + \ingroup group_ar_renderer + \defgroup group_ar_renderer_panda3d Panda3D Renderer + Modular renderers based on the Panda3D framework. +*/ +/*! + \ingroup group_ar_renderer_panda3d + \defgroup group_ar_renderer_panda3d_3d 3D scene renderers + Classes that render a 3D scene. +*/ +/*! + \ingroup group_ar_renderer_panda3d + \defgroup group_ar_renderer_panda3d_filters Image processing + Shader-based image processing and filtering. +*/ +/*! + \ingroup group_ar_renderer_panda3d + \defgroup group_ar_renderer_panda3d_lighting Lighting related classes + Light and Lightable implementations. +*/ + /*! \ingroup module_ar \defgroup group_ar_simulator Simulator diff --git a/doc/tutorial/rendering/tutorial-panda3d.dox b/doc/tutorial/rendering/tutorial-panda3d.dox new file mode 100644 index 0000000000..73787fa70a --- /dev/null +++ b/doc/tutorial/rendering/tutorial-panda3d.dox @@ -0,0 +1,267 @@ +/** +\page tutorial-panda3d Rendering a 3D scene with Panda3D +\tableofcontents + +\section tutorial-panda3d-intro Introduction + +In the context of providing a render-based tracker, this tutorial introduces a new, easy to use renderer based on +Panda3D. + +This renderer can output: +- A color image, with support for textures and lighting +- Depth image +- Normal maps + - In world space + - In camera space + +It only supports camera models with no distortion. + +It is also possible to compute camera clipping values, depending on the pose of an object in the camera frame. +This ensures that the depth buffer is as accurate as possible when considering this object. + +Below is a set of renders for a textured cube object. +\image html img-cube-rendering-panda3d.png + +Multi-output rendering is performed via the vpPanda3DRendererSet class, which duplicates the scene across multiple +renders and synchronizes changes to objects and the camera. Each Sub renderer implements a specific type of render: +geometric (vpPanda3DGeometryRenderer) or color-based (vpPanda3DRGBRenderer) etc. They all inherit from +vpPanda3DBaseRenderer, which implements basic functions for a panda renderer. + +\section tutorial-panda3d-install Panda3D installation +\subsection tutorial-panda3d-install-ubuntu Installation on Ubuntu + +- Installer are available for Ubuntu browsing the [download](https://www.panda3d.org/download/) page. + +- Hereafter you will find the instructions to build and install Panda3D from source on Ubuntu 22.04 + \code{.sh} + $ mkdir -p $VISP_WS/3rdparty/panda3d + $ cd $VISP_WS/3rdparty/panda3d + $ git clone https://github.com/panda3d/panda3d + $ cd panda3d + $ python3 makepanda/makepanda.py --everything --installer --no-egl --no-gles --no-gles2 --no-opencv + \endcode + At this point you can either: + 1. install the produced Debian package (recommended) with + \code{.sh} + $ sudo dpkg -i panda3d1.11_1.11.0_amd64.deb + \endcode + 2. use the Panda3D libraries located in the `built` folder without installing the Debian package + `panda3d1.11_1.11.0_amd64.deb`, but in that case you need to set `LD_LIBRARY_PATH` environment var: + \code{.sh} + $ export LD_LIBRARY_PATH=$VISP_WS/3rdparty/panda3d/panda3d/built/lib:$LD_LIBRARY_PATH + \endcode + Without setting `LD_LIBRARY_PATH` you may experience the following error when running a binary that uses + Panda3D capabilities: + \code{.sh} + $ ./tutorial-panda3d-renderer + ./tutorial-panda3d-renderer: error while loading shared libraries: libp3dtoolconfig.so.1.11: cannot open shared object file: No such file or directory + \endcode + +- Now to build ViSP with Panda3D support when Debian package `panda3d1.11_1.11.0_amd64.deb` is installed as described + in option (1), you may notice that there is nothing specific to do, just run `cmake` as usual: + \code{.sh} + $ cd $VISP_WS/visp-build + $ cmake ../visp + $ make -j$(nproc) + \endcode + +- There is also the possibility to build ViSP with Panda3D support without installing Debian package + `panda3d1.11_1.11.0_amd64.deb` as described in option (2): + - By setting `Panda3D_DIR` cmake var to the Panda3D cloned folder + \code{.sh} + $ cd $VISP_WS/visp-build + $ cmake ../visp -DPanda3D_DIR=$VISP_WS/3rdparty/panda3d/panda3d + $ make -j$(nproc) + \endcode + - By setting `Panda3D_DIR` environment variable + \code{.sh} + $ export Panda3D_DIR=$VISP_WS/3rdparty/panda3d/panda3d + $ cd $VISP_WS/visp-build + $ cmake ../visp + $ make -j$(nproc) + \endcode + +\subsection tutorial-panda3d-install-macos Installation on macOS + +- Installer are available for macOS browsing the [download](https://www.panda3d.org/download/) page. +\note For the latest Panda3D 1.10.14 SDK there is an `Installer for macOS X 10.9+` that is only compatible with + architecture `x86_64`. If you are using a Mac M1 or M2, there is no Panda3D SDK available yet for `arm64` + architecture. The solution is to build Panda3D from source. + +- Hereafter you will find the instructions to build Panda3D from source on macOS. + + - On macOS, you will need to download a set of precompiled third-party packages in order to compile Panda3D. + Navigate to PandaED [download page](https://www.panda3d.org/download/), select the lastest SDK + (in our case SDK 1.10.14), and under ` Source Code` section, download + [Thirdparty tools for macOS](https://www.panda3d.org/download/panda3d-1.10.14/panda3d-1.10.14-tools-mac.tar.gz) + (in our case `panda3d-1.10.14-tools-mac.tar.gz`). + - Extract third-party tools for macOS from downloaded archive + \code{.sh} + $ cd ~/Downloads + $ tar xvzf panda3d-1.10.14-tools-mac.tar.gz + \endcode + - Once done clone Panda3D: + \code{.sh} + $ mkdir -p $VISP_WS/3rdparty/panda3d + $ cd $VISP_WS/3rdparty/panda3d + $ git clone https://github.com/panda3d/panda3d + $ cd panda3d + \endcode + - Move the downloaded third-party tools in Panda3D source code folder + \code{.sh} + $ mv ~/Downloads/panda3d-1.10.14/thirdparty . + \endcode + - Build Panda3D from source + \code{.sh} + $ python3 makepanda/makepanda.py --everything --installer --no-egl --no-gles --no-gles2 --no-opencv --no-python --threads $(sysctl -n hw.logicalcpu) + \endcode + +- At this point you can either + 1. install the produced `Panda3D-1.11.0-py3.9.dmg` file (recommended) just by double clicking on it. In the + installer window, don't forget to enable the `C++ Header Files` check box before pressing the installation button. + After that you have to set `DYLIB_LIBRARY_PATH` environment var: + \code{.sh} + $ export DYLD_LIBRARY_PATH=/Library/Developer/Panda3D/lib:$DYLD_LIBRARY_PATH + \endcode + + 2. or use the Panda3D libraries located in the `built` folder without installing `.dmg` file, but in that case you + need to set `DYLIB_LIBRARY_PATH` environment var: + \code{.sh} + $ export DYLD_LIBRARY_PATH=$VISP_WS/3rdparty/panda3d/panda3d/built/lib:$DYLD_LIBRARY_PATH + \endcode + Without setting `DYLD_LIBRARY_PATH` you may experience the following error when running a binary that uses + Panda3D capabilities: + \code{.sh} + $ ./tutorial-panda3d-renderer + dyld[257]: Library not loaded: @loader_path/../lib/libpanda.1.11.dylib + \endcode + +- 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). + \code{.sh} + $ cd $VISP_WS/visp-build + $ cmake ../visp -DUSE_PCL=OFF + $ make -j$(sysctl -n hw.logicalcpu) + \endcode + +- There is also the possibility to build ViSP with Panda3D support without installing the `.dmg` file + - By setting `Panda3D_DIR` cmake var to the Panda3D cloned folder + \code{.sh} + $ cd $VISP_WS/visp-build + $ cmake ../visp -DUSE_PCL=OFF -DPanda3D_DIR=$VISP_WS/3rdparty/panda3d/panda3d + $ make -j$(sysctl -n hw.logicalcpu) + \endcode + + - Or by setting `Panda3D_DIR` environment variable + \code{.sh} + $ export Panda3D_DIR=$VISP_WS/3rdparty/panda3d/panda3d + $ cd $VISP_WS/visp-build + $ cmake ../visp -DUSE_PCL=OFF + $ make -j$(sysctl -n hw.logicalcpu) + \endcode + +\subsection tutorial-panda3d-install-windows Installation on Windows + +- Installer are available for Windows browsing the [download](https://www.panda3d.org/download/) page. + +\section tutorial-panda3d-usage Rendere based on Panda3D usage + +An example that shows how to exploit Panda3D in ViSP to render a color image with support for textures and lighting, a +depth image, normals in world space and in camera space is given in tutorial-panda3d-renderer.cpp. + +Here you will find the code used to create the renderer: +\snippet tutorial-panda3d-renderer.cpp Renderer set + +Here you will find the code used to create the sub renderers: +\snippet tutorial-panda3d-renderer.cpp Subrenderers init + +Here you will find the code used to add the sub renderers to the main renderer: +\snippet tutorial-panda3d-renderer.cpp Adding subrenderers + +Here you will find the code used to configure the scene: +\snippet tutorial-panda3d-renderer.cpp Scene configuration + +\section tutorial-panda3d-full-code Tutorial full code + +The full code of tutorial-panda3d-renderer.cpp is given below. +\include tutorial-panda3d-renderer.cpp + +\section tutorial-panda3d-run Execute the tutorial + +- Once ViSP is build, you may run the tutorial by: + \code{.sh} + $ cd $VISP_WS/visp-build + $ ./tutorial/ar/tutorial-panda3d-renderer + \endcode + It downloads the object located by default in `tutorial/ar/data/suzanne.bam` file. +- You should see something similar to the following video +\htmlonly +

+\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 + ^ + 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) + ``` +*/ diff --git a/doc/tutorial/tutorial-users.dox b/doc/tutorial/tutorial-users.dox index fb1db6ceb5..20ae9ff505 100644 --- a/doc/tutorial/tutorial-users.dox +++ b/doc/tutorial/tutorial-users.dox @@ -11,6 +11,8 @@ This page references all the tutorials to use and contribute to ViSP. - \subpage tutorial_calib +- \subpage tutorial_rendering + - \subpage tutorial_tracking - \subpage tutorial_detection @@ -85,6 +87,12 @@ This page introduces the user to the way to calibrate a camera. - \subpage tutorial-calibration-extrinsic
This tutorial explains how to get the camera eye-in-hand extrinsic transformation when the camera is mounted on a robot end-effector. */ +/*! \page tutorial_rendering 3D rendering and augmented reality +This page gives an introduction to using the 3D rendering tools integrated into ViSP. + +- \subpage tutorial-panda3d
This tutorial gives an overview of the rendering capabilities of the Panda3D wrapper. +*/ + /*! \page tutorial_tracking Tracking This page introduces the user to the way to track objects in images. diff --git a/modules/ar/CMakeLists.txt b/modules/ar/CMakeLists.txt index 004b702c55..4185627be7 100644 --- a/modules/ar/CMakeLists.txt +++ b/modules/ar/CMakeLists.txt @@ -1,7 +1,7 @@ ############################################################################# # # ViSP, open source Visual Servoing Platform software. -# Copyright (C) 2005 - 2023 by Inria. All rights reserved. +# 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 @@ -173,6 +173,15 @@ if(USE_COIN3D) endif() endif() +if(USE_PANDA3D) + if(Panda3D_INCLUDE_DIRS) + list(APPEND opt_incs ${Panda3D_INCLUDE_DIRS}) + endif() + if(Panda3D_LIBRARIES) + list(APPEND opt_libs ${Panda3D_LIBRARIES}) + endif() +endif() + vp_add_module(ar visp_core) vp_glob_module_sources() @@ -181,5 +190,17 @@ if(USE_OGRE) vp_set_source_file_compile_flag(src/ogre-simulator/vpAROgre.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual -Wno-float-equal -Wno-deprecated-copy -Wno-register) endif() +if(USE_PANDA3D) + set(PANDA3D_CXX_FLAGS -Wno-unused-parameter -Wno-unused-variable -Wno-extra -Wno-reorder-ctor) + set(PANDA3D_MODULE_SOURCES + vpPanda3DBaseRenderer.cpp vpPanda3DGeometryRenderer.cpp + vpPanda3DRGBRenderer.cpp vpPanda3DRenderParameters.cpp + vpPanda3DRendererSet.cpp vpPanda3DPostProcessFilter.cpp vpPanda3DCommonFilters.cpp + ) + foreach(panda_src_name ${PANDA3D_MODULE_SOURCES}) + vp_set_source_file_compile_flag(src/panda3d-simulator/${panda_src_name} ${PANDA3D_CXX_FLAGS}) + endforeach() +endif() + vp_module_include_directories(${opt_incs}) vp_create_module(${opt_libs}) diff --git a/modules/ar/include/visp3/ar/vpAR.h b/modules/ar/include/visp3/ar/vpAR.h index da0f5585c6..cb110926cd 100644 --- a/modules/ar/include/visp3/ar/vpAR.h +++ b/modules/ar/include/visp3/ar/vpAR.h @@ -1,7 +1,7 @@ -/**************************************************************************** +/* * * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,7 @@ * Description: * Use to display an image behind the internal view of the simulator * used for augmented reality application - * -*****************************************************************************/ + */ /*! \file vpAR.h @@ -45,8 +44,8 @@ */ -#ifndef vpAR_HH -#define vpAR_HH +#ifndef VP_AR_H +#define VP_AR_H #include @@ -84,58 +83,57 @@ BEGIN_VISP_NAMESPACE The code below shows how to use the class. \code -#include -#include -#include -#include + #include + #include + #include + #include -#ifdef VISP_HAVE_COIN3D_AND_GUI -static void *mainloopfunction(void *_simu) -{ - vpAR *simu = (vpAR *)_simu ; - simu->initMainApplication() ; + #ifdef VISP_HAVE_COIN3D_AND_GUI + static void *mainloopfunction(void *_simu) + { + vpAR *simu = (vpAR *)_simu ; + simu->initMainApplication() ; - vpImage I; - vpHomogeneousMatrix cMo; + vpImage I; + vpHomogeneousMatrix cMo; - //Your code to compute the pose cMo. + //Your code to compute the pose cMo. - //Set the image to use as background. - simu->setImage(I) ; - //Set the camera position thanks to the pose cMo computed before. - simu->setCameraPosition(cMo) ; + //Set the image to use as background. + simu->setImage(I) ; + //Set the camera position thanks to the pose cMo computed before. + simu->setCameraPosition(cMo) ; - simu->closeMainApplication(); -} -#endif + simu->closeMainApplication(); + } + #endif -int main() -{ -#ifdef VISP_HAVE_COIN3D_AND_GUI - vpAR simu; - //Camera parameters. - vpCameraParameters cam(600,600,160,120); + int main() + { + #ifdef VISP_HAVE_COIN3D_AND_GUI + vpAR simu; + //Camera parameters. + vpCameraParameters cam(600,600,160,120); - //Initialize the internal view of the simulator. - simu.initInternalViewer(640,480, vpSimulator::grayImage); + //Initialize the internal view of the simulator. + simu.initInternalViewer(640,480, vpSimulator::grayImage); - vpTime::wait(300); + vpTime::wait(300); - // Load the cad model. 4points.iv can be downloaded on the website - // with the image package - simu.load("./4points.iv"); + // Load the cad model. 4points.iv can be downloaded on the website + // with the image package + simu.load("./4points.iv"); - //Initialize the internal camera parameters. - simu.setInternalCameraParameters(cam); + //Initialize the internal camera parameters. + simu.setInternalCameraParameters(cam); - simu.initApplication(&mainloopfunction); + simu.initApplication(&mainloopfunction); - simu.mainLoop(); -#endif - return 0; -} + simu.mainLoop(); + #endif + return 0; + } \endcode - */ class VISP_EXPORT vpAR : public vpSimulator { diff --git a/modules/ar/include/visp3/ar/vpAROgre.h b/modules/ar/include/visp3/ar/vpAROgre.h index 62046d5505..87c2baa8ad 100644 --- a/modules/ar/include/visp3/ar/vpAROgre.h +++ b/modules/ar/include/visp3/ar/vpAROgre.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Augmented Reality viewer using Ogre3D. - * - * Authors: - * Bertrand Delabarre - * -*****************************************************************************/ + */ /*! \file vpAROgre.h @@ -46,8 +41,8 @@ */ -#ifndef _vpAROgre_h_ -#define _vpAROgre_h_ +#ifndef VP_AR_OGRE_H +#define VP_AR_OGRE_H #include diff --git a/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h new file mode 100644 index 0000000000..bd02273272 --- /dev/null +++ b/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h @@ -0,0 +1,294 @@ +/* + * 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. + */ + +#ifndef VP_PANDA3D_BASE_RENDERER_H +#define VP_PANDA3D_BASE_RENDERER_H + +#include + +#if defined(VISP_HAVE_PANDA3D) +#include +#include +#include + +#include +#include + +BEGIN_VISP_NAMESPACE +/** + * \ingroup group_ar_renderer_panda3d + * + * \brief Base class for a panda3D renderer. This class handles basic functionalities, + * such as loading object, changing camera parameters. + * + * For a subclass to have a novel behaviour (e.g, display something else) These methods should be overriden: + * + * - setupScene: This is where you should apply your shaders. + * - setupCamera: This is where cameras are created and intrinsics parameters are applied + * - setupRenderTarget: This is where you should create the texture buffers, where the render results should be stored. +*/ +class VISP_EXPORT vpPanda3DBaseRenderer +{ +public: + vpPanda3DBaseRenderer(const std::string &rendererName) + : m_name(rendererName), m_renderOrder(-100), m_framework(nullptr), m_window(nullptr), m_camera(nullptr) + { + setVerticalSyncEnabled(false); + } + + virtual ~vpPanda3DBaseRenderer() = default; + + /** + * @brief Initialize the whole Panda3D framework. Create a new PandaFramework object and a new window. + * + * 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, PT(WindowFramework) window); + + + virtual void renderFrame(); + + /** + * @brief Get the name of the renderer + * + * @return const std::string& + */ + const std::string &getName() const { return m_name; } + + /** + * @brief Get the scene root + * + */ + NodePath &getRenderRoot() { return m_renderRoot; } + + /** + * @brief Set new rendering parameters. If the scene has already been initialized, the renderer camera is updated. + * + * @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); + } + } + + /** + * @brief Returns true if this renderer process 3D data and its scene root can be interacted with. + * + * This value could be false, if for instance it is redefined in a subclass that performs postprocessing on a texture. + */ + virtual bool isRendering3DScene() const { return true; } + + /** + * @brief Get the rendering order of this renderer. + * If a renderer A has a lower order value than B, it will be rendered before B. + * This is useful, if for instance, B is a postprocessing filter that depends on the result of B. + * + * @return int + */ + int getRenderOrder() const { return m_renderOrder; } + + /** + * @brief Set the camera's pose. + * The pose is specified using the ViSP convention (Y-down right handed). + * + * @param wTc the new pose of the camera, in world frame + */ + virtual void setCameraPose(const vpHomogeneousMatrix &wTc); + + /** + * @brief Retrieve the camera's pose, in the world frame. + * The pose is specified using the ViSP convention (Y-down right handed). + */ + virtual vpHomogeneousMatrix getCameraPose(); + + /** + * @brief Set the pose of a node. This node can be any Panda object (light, mesh, camera). + * The pose is specified using the ViSP convention (Y-down right handed). + * + * @param name Node path to search for, from the render root. This is the object that will be modified See https://docs.panda3d.org/1.10/python/programming/scene-graph/searching-scene-graph + * @param wTo Pose of the object in the world frame + * + * \throws if the corresponding node cannot be found. + */ + virtual void setNodePose(const std::string &name, const vpHomogeneousMatrix &wTo); + + /** + * @brief Set the pose of a node. + * The pose is specified using the ViSP convention (Y-down right handed). + * This node can be any Panda object (light, mesh, camera). + * + * @param object The object for which to set the pose + * @param wTo Pose of the object in the world frame + */ + virtual void setNodePose(NodePath &object, const vpHomogeneousMatrix &wTo); + + /** + * @brief Get the pose of a Panda node, in world frame in the ViSP convention (Y-down right handed). + * + * @param name Node path to search for. \see setNodePose(const std::string &, const vpHomogeneousMatrix &) for more info + * @return wTo, the pose of the object in world frame + * \throws if no node can be found from the given path. + */ + virtual vpHomogeneousMatrix getNodePose(const std::string &name); + + /** + * @brief Get the pose of a Panda node, in world frame in the ViSP convention (Y-down right handed). This version of the method directly uses the Panda Nodepath. + */ + virtual vpHomogeneousMatrix getNodePose(NodePath &object); + + /** + * @brief Compute the near and far planes for the camera at the current pose, given a certain node/part of the graph. + * + * The near clipping value will be set to the distance to the closest point of the object. + * The far clipping value will be set to the distance to farthest vertex of the object. + * + * \warning Depending on geometry complexity, this may be an expensive operation. + * \warning if the object lies partly behind the camera, the near plane value will be zero. + * If it fully behind, the far plane will also be zero. If these near/far values are used to update the + * rendering parameters of the camera, this may result in an invalid projection matrix. + * + * @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 + */ + void computeNearAndFarPlanesFromNode(const std::string &name, float &near, float &far); + + /** + * @brief Load a 3D object. To load an .obj file, Panda3D must be compiled with assimp support. + * + * Once loaded, the object will not be visible, it should be added to the scene. + * + * @param nodeName the name that will be used when inserting the node in the scene graph + * @param modelPath Path to the model file + * @return NodePath The NodePath containing the 3D model, which can now be added to the scene graph. + */ + NodePath loadObject(const std::string &nodeName, const std::string &modelPath); + + /** + * @brief Add a node to the scene. Its pose is set as the identity matrix + * + * @param object + */ + virtual void addNodeToScene(const NodePath &object); + + /** + * @brief set whether vertical sync is enabled. + * When vertical sync is enabled, render speed will be limited by the display's refresh rate + * + * @param useVsync Whether to use vsync or not + */ + void setVerticalSyncEnabled(bool useVsync); + /** + * @brief Set the behaviour when a Panda3D assertion fails. If abort is true, the program will stop. + * Otherwise, an error will be displayed in the console. + * + * @param abort whether to abort (true) or display a message when an assertion fails. + */ + void setAbortOnPandaError(bool abort); + void enableDebugLog(); + + static vpColVector vispPointToPanda(const vpColVector &point); + static vpColVector vispVectorToPanda(const vpColVector &vec); + + void printStructure(); + + virtual GraphicsOutput *getMainOutputBuffer() { return nullptr; } + +protected: + + /** + * @brief Initialize the scene for this specific renderer. + * + * Creates a root scene for this node and applies shaders. that will be used for rendering + * + */ + virtual void setupScene(); + + /** + * @brief Initialize camera. Should be called when the scene root of this render has already been created. + * + */ + virtual void setupCamera(); + + /** + * @brief Initialize buffers and other objects that are required to save the render. + * + */ + virtual void setupRenderTarget() { } + + + const static vpHomogeneousMatrix VISP_T_PANDA; //! Homogeneous transformation matrix to convert from the Panda coordinate system (right-handed Z-up) to the ViSP coordinate system (right-handed Y-Down) + const static vpHomogeneousMatrix PANDA_T_VISP; //! Inverse of VISP_T_PANDA + + +protected: + const std::string m_name; //! name of the renderer + int m_renderOrder; //! Rendering priority for this renderer and its buffers. A lower value will be rendered first. Should be used when calling make_output in setupRenderTarget() + std::shared_ptr m_framework; //! Pointer to the active panda framework + PT(WindowFramework) m_window; //! Pointer to owning window, which can create buffers etc. It is not necessarily visible. + vpPanda3DRenderParameters m_renderParameters; //! Rendering parameters + NodePath m_renderRoot; //! Node containing all the objects and the camera for this renderer + PT(Camera) m_camera; + NodePath m_cameraPath; //! NodePath of the camera + std::vector m_buffers; //! Set of buffers that this renderer uses. This storage contains weak refs to those buffers and should not deallocate them. +}; + +END_VISP_NAMESPACE +#endif //VISP_HAVE_PANDA3D +#endif diff --git a/modules/ar/include/visp3/ar/vpPanda3DCommonFilters.h b/modules/ar/include/visp3/ar/vpPanda3DCommonFilters.h new file mode 100644 index 0000000000..5b1e40016f --- /dev/null +++ b/modules/ar/include/visp3/ar/vpPanda3DCommonFilters.h @@ -0,0 +1,105 @@ +/* + * 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. + */ + +#ifndef VP_PANDA3D_COMMON_FILTERS_H +#define VP_PANDA3D_COMMON_FILTERS_H + +#include + +#if defined(VISP_HAVE_PANDA3D) + +#include + +BEGIN_VISP_NAMESPACE +class vpPanda3DRGBRenderer; + +/** + * \ingroup group_ar_renderer_panda3d_filters + * \brief Class that implements an RGB to grayscale conversion. + * + */ +class VISP_EXPORT vpPanda3DLuminanceFilter : public vpPanda3DPostProcessFilter +{ +public: + vpPanda3DLuminanceFilter(const std::string &name, std::shared_ptr inputRenderer, bool isOutput); + FrameBufferProperties getBufferProperties() const vp_override; + void getRender(vpImage &I) const; + +private: + static const char *FRAGMENT_SHADER; +}; + +/** + * + * \ingroup group_ar_renderer_panda3d_filters + * \brief Class that implements a gaussian filter on a grayscale image. + * The grayscale image should be contained in the blue channel of the image. + * + */ +class VISP_EXPORT vpPanda3DGaussianBlur : public vpPanda3DPostProcessFilter +{ +public: + vpPanda3DGaussianBlur(const std::string &name, std::shared_ptr inputRenderer, bool isOutput); + FrameBufferProperties getBufferProperties() const vp_override; + void getRender(vpImage &I) const; + +private: + static const char *FRAGMENT_SHADER; +}; + +/** + * \ingroup group_ar_renderer_panda3d_filters + * \brief Implementation of canny filtering, using Sobel kernels. + * + * The results of the canny are filtered based on a threshold value (defined between 0 and 255), checking whether there is enough gradient information. + * The output of this image is a floating RGB image containing: + * - In the red channel, the value of the convolution with the sobel horizontal kernel + * - In the green channel, the value of the convolution with the sobel vertical kernel + * - In the blue channel, the angle (in radians) of the edge normal. + */ +class VISP_EXPORT vpPanda3DCanny : public vpPanda3DPostProcessFilter +{ +public: + vpPanda3DCanny(const std::string &name, std::shared_ptr inputRenderer, bool isOutput, float edgeThreshold); + FrameBufferProperties getBufferProperties() const vp_override; + void getRender(vpImage &I) const; + void setEdgeThreshold(float edgeThreshold); + +protected: + void setupScene() vp_override; + +private: + static const char *FRAGMENT_SHADER; + float m_edgeThreshold; +}; + +END_VISP_NAMESPACE +#endif +#endif diff --git a/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h new file mode 100644 index 0000000000..b15e192f8b --- /dev/null +++ b/modules/ar/include/visp3/ar/vpPanda3DGeometryRenderer.h @@ -0,0 +1,103 @@ +/* + * 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. + */ + +#ifndef VP_PANDA3D_GEOMETRY_RENDERER_H +#define VP_PANDA3D_GEOMETRY_RENDERER_H + +#include + +#if defined(VISP_HAVE_PANDA3D) +#include +#include +#include + +BEGIN_VISP_NAMESPACE +/** + * \ingroup group_ar_renderer_panda3d_3d + * + * @brief Renderer that outputs object geometric information. + * + * This information may contain, depending on requested render type: + * + * - Normals in the world frame or in the camera frame. + * - Depth information +*/ +class VISP_EXPORT vpPanda3DGeometryRenderer : public vpPanda3DBaseRenderer +{ +public: + + enum vpRenderType + { + OBJECT_NORMALS, //! Surface normals in the object frame. + CAMERA_NORMALS, //! Surface normals in the frame of the camera. Z points towards the camera and y is up. + }; + + vpPanda3DGeometryRenderer(vpRenderType renderType); + ~vpPanda3DGeometryRenderer() { } + + /** + * @brief Get render results into ViSP readable structures + * + * + * @param colorData Depending on the vpRenderType, normals in the world or camera frame may be stored in this image. + * @param depth Image used to store depth + */ + void getRender(vpImage &colorData, vpImage &depth) 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. + */ + void getRender(vpImage &colorData) const; + /** + * @brief Get render results into ViSP readable structures. This version only retrieves the depth data. + * @param depth Depending on the vpRenderType, rendered depth may be stored in this image. + */ + void getRender(vpImage &depth) const; + + GraphicsOutput *getMainOutputBuffer() vp_override { return m_normalDepthBuffer; } + +protected: + void setupScene() vp_override; + void setupRenderTarget() vp_override; + +private: + vpRenderType m_renderType; + Texture *m_normalDepthTexture; + GraphicsOutput *m_normalDepthBuffer; + + static const char *SHADER_VERT_NORMAL_AND_DEPTH_WORLD; + static const char *SHADER_VERT_NORMAL_AND_DEPTH_CAMERA; + static const char *SHADER_FRAG_NORMAL_AND_DEPTH; + +}; +END_VISP_NAMESPACE +#endif //VISP_HAVE_PANDA3D +#endif diff --git a/modules/ar/include/visp3/ar/vpPanda3DLight.h b/modules/ar/include/visp3/ar/vpPanda3DLight.h new file mode 100644 index 0000000000..f225a29390 --- /dev/null +++ b/modules/ar/include/visp3/ar/vpPanda3DLight.h @@ -0,0 +1,275 @@ +/* + * 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. + */ + +#ifndef VP_PANDA3D_LIGHT_H +#define VP_PANDA3D_LIGHT_H + +#include + +#if defined(VISP_HAVE_PANDA3D) + +#include +#include +#include +#include + +#include "nodePath.h" +#include "ambientLight.h" +#include "directionalLight.h" +#include "pointLight.h" +#include "directionalLight.h" + +BEGIN_VISP_NAMESPACE +/** + * \ingroup group_ar_renderer_panda3d_lighting + * + * \brief Base class for a Light that can be added to a Panda3D scene. + * + * Note that modifying any object that inherits from this class + * after the method addToScene has been called * will not update the rendered light. + * + * \see https://docs.panda3d.org/1.10/cpp/programming/render-attributes/lighting + * +*/ +class VISP_EXPORT vpPanda3DLight +{ +public: + /** + * \brief Build a new Panda3D light, given a unique name and an RGB color. + * + * + * \param name the name of the light: should be unique in the scene where the light will be added. + * \param color The color of the light: Each R,G,B component is unbounded and can exceed a value of 1 to increase its intensity. + */ + vpPanda3DLight(const std::string &name, const vpRGBf &color) : m_name(name), m_color(color) { } + + /** + * \brief Get the name of the light. + * + * This name should be unique and will be required when interacting with Panda3D to fetch the node. + */ + const std::string &getName() const { return m_name; } + /** + * \brief Get the light's color + * + * \return const vpRGBf& + */ + const vpRGBf &getColor() const { return m_color; } + + /** + * \brief Add the light to the scene. + * + * \param scene Scene where the light should be added. + */ + virtual void addToScene(NodePath &scene) const = 0; + +protected: + std::string m_name; //! Name of the light. Should be unique in the scene + vpRGBf m_color; //! RGB Color of the light. Can exceed 1 for each component. +}; + +/** + * + * \ingroup group_ar_renderer_panda3d_lighting + * + * \brief Class representing an ambient light. + * + * Ambient light are not physically possible, but are used to emulate light coming from all directions. + * They do not generate speculars or reflections. + * + * \see https://docs.panda3d.org/1.10/python/reference/panda3d.core.AmbientLight + */ +class VISP_EXPORT vpPanda3DAmbientLight : public vpPanda3DLight +{ +public: + vpPanda3DAmbientLight(const std::string &name, const vpRGBf &color) : vpPanda3DLight(name, color) { } + + void addToScene(NodePath &scene) const vp_override + { + PT(AmbientLight) light = new AmbientLight(m_name); + light->set_color(LColor(m_color.R, m_color.G, m_color.B, 1)); + NodePath alnp = scene.attach_new_node(light); + scene.set_light(alnp); + } +}; + +/** + * \ingroup group_ar_renderer_panda3d_lighting + * + * \brief Class representing a Point Light. + * + * Point lights emit light all around them, from a single point. + * Their light can be subject to a distance-based attenuation. + */ +class VISP_EXPORT vpPanda3DPointLight : public vpPanda3DLight +{ +public: + /** + * \brief Build a new point light. + * + * \see vpPanda3DLight constructor. + * + * \param name name of the light + * \param color color of the light + * \param position Position in the scene of the light. Uses ViSP coordinates. + * \param attenuation Attenuation components of the light as a function of distance. + * Should be a vector of size 3 where the first component is the constant intensity factor (no falloff), + * the second is a linear falloff coefficient, and the last one is the quadratic falloff component. + * To follow the inverse square law, set this value vector to [0, 0, 1] + * To have no falloff, set it to [1, 0, 0]. + */ + vpPanda3DPointLight(const std::string &name, const vpRGBf &color, const vpColVector &position, const vpColVector &attenuation) + : vpPanda3DLight(name, color), m_attenuation(attenuation) + { + if (position.size() != 3) { + throw vpException(vpException::dimensionError, "Point light position must be a 3 dimensional vector"); + } + m_position.resize(4, false); + m_position.insert(0, position); + m_position[3] = 1.0; + if (attenuation.size() != 3) { + throw vpException(vpException::dimensionError, "Point light attenuation components must be a 3 dimensional vector"); + } + } + + void addToScene(NodePath &scene) const vp_override + { + PT(PointLight) light = new PointLight(m_name); + light->set_color(LColor(m_color.R, m_color.G, m_color.B, 1)); + light->set_attenuation(LVecBase3(m_attenuation[0], m_attenuation[1], m_attenuation[2])); + NodePath np = scene.attach_new_node(light); + //vpColVector posPanda = vpPanda3DBaseRenderer::vispPointToPanda(m_position); + np.set_pos(m_position[0], m_position[1], m_position[2]); + scene.set_light(np); + } + +private: + vpColVector m_position; //! Position of the light, in homogeneous coordinates + vpColVector m_attenuation; //! Attenuation components: [constant, linear, quadratic] +}; +/** + * + * + * \ingroup group_ar_renderer_panda3d_lighting + * \brief Class representing a directional light + * + * A directional light has no origin nor falloff. + * + */ +class VISP_EXPORT vpPanda3DDirectionalLight : public vpPanda3DLight +{ +public: + /** + * \brief Build a new directional light. + * + * \see vpPanda3DLight constructor. + * + * \param name name of the light + * \param color color of the light + * \param direction Position in the scene of the light. Uses ViSP coordinates. + */ + vpPanda3DDirectionalLight(const std::string &name, const vpRGBf &color, const vpColVector &direction) + : vpPanda3DLight(name, color), m_direction(direction) + { + if (m_direction.size() != 3) { + throw vpException(vpException::dimensionError, "Direction light direction must be a 3 dimensional vector"); + } + m_direction.normalize(); + } + + void addToScene(NodePath &scene) const vp_override + { + 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); + } + +private: + vpColVector m_direction; //! Direction vector of the light, in scene frame +}; + +/** + * \ingroup group_ar_renderer_panda3d_lighting + * \brief Interface for objects, scenes or other Panda3D related data that can be lit by a vpPanda3DLight. + * + */ +class VISP_EXPORT vpPanda3DLightable +{ +public: + virtual ~vpPanda3DLightable() = default; + /** + * \brief Light this lightable object with a new light + * + * \param light + */ + virtual void addLight(const vpPanda3DLight &light) = 0; +}; +/** + * \ingroup group_ar_renderer_panda3d_lighting + * \brief Implementation of vpPanda3DLightable for a panda scene with a root node. + * + * The root node should be specified with setLightableScene by an inheriting implementation. + */ +class VISP_EXPORT vpPanda3DLightableScene : public vpPanda3DLightable +{ +public: + vpPanda3DLightableScene() : vpPanda3DLightable() + { } + + vpPanda3DLightableScene(NodePath &scene) : vpPanda3DLightable(), m_lightableScene(scene) + { } + + /** + * \brief Add a light to the scene. All of the objects in the scene will be lit. + * + * \throws if the scene is not setup (setLightableScene or constructor with NodePath has not been called) + * \param light light to add + */ + void addLight(const vpPanda3DLight &light) vp_override + { + if (m_lightableScene.is_empty()) { + throw vpException(vpException::notInitialized, "Tried to add a light to a scene that is not initialized."); + } + light.addToScene(m_lightableScene); + } +protected: + void setLightableScene(NodePath &scene) { m_lightableScene = scene; } +private: + NodePath m_lightableScene; //! Scene that should be lit when calling addLight +}; + +END_VISP_NAMESPACE + +#endif +#endif diff --git a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h new file mode 100644 index 0000000000..c5986d2379 --- /dev/null +++ b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h @@ -0,0 +1,98 @@ +/* + * 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. + */ + +#ifndef VP_PANDA3D_POST_PROCESS_FILTER_H +#define VP_PANDA3D_POST_PROCESS_FILTER_H + +#include + +#if defined(VISP_HAVE_PANDA3D) +#include +#include "cardMaker.h" +#include "orthographicLens.h" + +BEGIN_VISP_NAMESPACE +/** + * \ingroup group_ar_renderer_panda3d_filters + * \brief Base class for postprocessing filters that map the result of a vpPanda3DBaseRenderer to a new image. + * + * Unlike 3D renderers, implementations of this class do not have access to 3D information + * (except if it is the result of the processed image). + * + * Implementation wise, the process is the following: + * + * - The output texture (retrieved through vpPanda3DBaseRenderer::getMainOutputBuffer) is blitted on a quad, + * that is placed perfectly in front of the camera. + * - A shader (given as an argument to the constructor) is applied to this quad. + * - The result is copied back to ram if required. +*/ +class VISP_EXPORT vpPanda3DPostProcessFilter : public vpPanda3DBaseRenderer +{ +public: + vpPanda3DPostProcessFilter(const std::string &name, std::shared_ptr inputRenderer, bool isOutput, std::string fragmentShader) + : vpPanda3DBaseRenderer(name), m_inputRenderer(inputRenderer), m_isOutput(isOutput), m_fragmentShader(fragmentShader) + { + m_renderOrder = m_inputRenderer->getRenderOrder() + 1; + } + + bool isRendering3DScene() const vp_override + { + return false; + } + + GraphicsOutput *getMainOutputBuffer() vp_override { return m_buffer; } + +protected: + virtual void setupScene() vp_override; + + void setupCamera() vp_override; + + void setupRenderTarget() vp_override; + + void setRenderParameters(const vpPanda3DRenderParameters ¶ms) vp_override; + + void getRenderBasic(vpImage &I) const; + void getRenderBasic(vpImage &I) const; + + + virtual FrameBufferProperties getBufferProperties() const = 0; + + std::shared_ptr m_inputRenderer; + bool m_isOutput; //! Whether this filter is an output to be used and should be copied to ram + std::string m_fragmentShader; + PT(Shader) m_shader; + Texture *m_texture; + GraphicsOutput *m_buffer; + + static const char *FILTER_VERTEX_SHADER; +}; +END_VISP_NAMESPACE +#endif +#endif diff --git a/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h new file mode 100644 index 0000000000..8132974df2 --- /dev/null +++ b/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h @@ -0,0 +1,120 @@ +/* + * 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. + */ + +#ifndef VP_PANDA3D_RGB_RENDERER_H +#define VP_PANDA3D_RGB_RENDERER_H + +#include + +#if defined(VISP_HAVE_PANDA3D) + +#include +#include +#include + +BEGIN_VISP_NAMESPACE +/** + * \ingroup group_ar_renderer_panda3d_3d + * \brief Implementation of a traditional RGB renderer in Panda3D + * + * The lighting model follows a Cook-torrance BRDF. + * + * For each object, a specific Version of the cook-torrance shader is compiled: diffuse textures are supported, but normal/bump/roughness maps are not. + * This class will try to automatically detect whether an object has RGB textures. + * + * Specular highlights and reflections can be ignored, depending on the value of isShowingSpeculars. + * + * \warning if an object is detected as having image textures but it actually doesn't have any, the object may appear washed out. + * + * \note Most of the tested objects were in BAM format, Panda3D's own format. + * The following pipeline was used: + * - Export to GLTF with Blender + * - In a Python environment, install gltf2bam with `pip install panda3d-gltf` + * - run gltf2bam path/to/yourObject.gltf path/to/yourObject.bam + * - then, in the code, use `renderer.addNodeToScene("/path/to/yourObject.bam");` + * +*/ +class VISP_EXPORT vpPanda3DRGBRenderer : public vpPanda3DBaseRenderer, public vpPanda3DLightableScene +{ +public: + /** + * \brief Default constructor. Initialize an RGB renderer with the normal rendering behavior showing speculars + * + */ + vpPanda3DRGBRenderer() : vpPanda3DBaseRenderer("RGB"), m_showSpeculars(true), m_display2d(nullptr), m_backgroundTexture(nullptr) { } + /** + * \brief RGB renderer constructor allowing to specify + * whether specular highlights should be rendered or + * if only ambient/diffuse lighting should be considered. + * + * \param showSpeculars whether to render speculars + */ + vpPanda3DRGBRenderer(bool showSpeculars) : vpPanda3DBaseRenderer(showSpeculars ? "RGB" : "RGB-diffuse"), m_showSpeculars(showSpeculars) { } + + + /** + * @brief Store the render resulting from calling renderFrame() into a vpImage. + * + * If the image does not have the correct dimensions, it is resized. + * + * @param I The image in which to store the render. + */ + void getRender(vpImage &I) const; + + void addNodeToScene(const NodePath &object) vp_override; + + void setBackgroundImage(const vpImage &background); + + GraphicsOutput *getMainOutputBuffer() vp_override { return m_colorBuffer; } + + bool isShowingSpeculars() const { return m_showSpeculars; } + + +protected: + void setupScene() vp_override; + void setupRenderTarget() vp_override; + virtual std::string makeFragmentShader(bool hasTexture, bool specular); + +private: + bool m_showSpeculars; + Texture *m_colorTexture; + GraphicsOutput *m_colorBuffer; + static const char *COOK_TORRANCE_VERT; + static const char *COOK_TORRANCE_FRAG; + + NodePath m_backgroundImage; + DisplayRegion *m_display2d; + PT(Texture) m_backgroundTexture; + +}; + +END_VISP_NAMESPACE +#endif //VISP_HAVE_PANDA3D +#endif diff --git a/modules/ar/include/visp3/ar/vpPanda3DRenderParameters.h b/modules/ar/include/visp3/ar/vpPanda3DRenderParameters.h new file mode 100644 index 0000000000..3a4cf51584 --- /dev/null +++ b/modules/ar/include/visp3/ar/vpPanda3DRenderParameters.h @@ -0,0 +1,129 @@ +/* + * 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. + */ + +#ifndef VP_PANDA3D_RENDER_PARAMETERS_H +#define VP_PANDA3D_RENDER_PARAMETERS_H + +#include + +#if defined(VISP_HAVE_PANDA3D) +#include + +class Camera; + +BEGIN_VISP_NAMESPACE +/** + * @brief Rendering parameters for a panda3D simulation + * + * includes: + * - Camera intrinsics + * - Image resolution + * - Clipping parameters +*/ +class VISP_EXPORT vpPanda3DRenderParameters +{ +public: + vpPanda3DRenderParameters() : m_cam(), m_height(0), m_width(0), m_clipNear(0.001), m_clipFar(10.0) { } + vpPanda3DRenderParameters(const vpCameraParameters &cam, unsigned int h, unsigned int w, + double clipNear, double clipFar) + : m_cam(cam), m_height(h), m_width(w), m_clipNear(clipNear), m_clipFar(clipFar) + { } + + /** + * @brief Retrieve camera intrinsics. + * + * @return const vpCameraParameters& + */ + const vpCameraParameters &getCameraIntrinsics() const { return m_cam; } + /** + * @brief set camera intrinsics. Only camera intrinsics for a lens without distortion are supported. + * \throws if camera intrinsics have a distortion model. + */ + void setCameraIntrinsics(const vpCameraParameters &cam) + { + if (cam.get_projModel() != vpCameraParameters::perspectiveProjWithoutDistortion) { + throw vpException(vpException::badValue, "Panda3D renderer: only lenses with no distortion are supported"); + } + m_cam = cam; + } + + double getNearClippingDistance() const { return m_clipNear; } + double getFarClippingDistance() const { return m_clipFar; } + + /** + * @brief Set the clipping distance. When a panda camera uses these render parameters, objects that are closer than "near" or further than "far" will be clipped. + * + * @param nearV near clipping distance + * @param farV far clipping distance + */ + void setClippingDistance(double nearV, double farV) + { + if (farV < nearV) { + std::swap(nearV, farV); + } + m_clipNear = nearV; + m_clipFar = farV; + } + + unsigned int getImageWidth() const { return m_width; } + unsigned int getImageHeight() const { return m_height; } + + /** + * @brief Set the image resolution. + * When this object is given to a vpPanda3DBaseRenderer, + * this will be the resolution of the renderer's output images. + * + * @param height vertical image resolution + * @param width horizontal image resolution + */ + void setImageResolution(unsigned int height, unsigned int width) + { + m_height = height; + m_width = width; + } + + /** + * @brief Update a Panda3D camera object to use this objects's parameters. + * + * @param camera the camera for which to update the rendering parameters + * + * \throws if getImageWidth() or getImageHeight() are equal to 0. + */ + void setupPandaCamera(Camera *camera); + +private: + vpCameraParameters m_cam; + unsigned int m_height, m_width; + double m_clipNear, m_clipFar; +}; + +END_VISP_NAMESPACE +#endif +#endif diff --git a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h new file mode 100644 index 0000000000..dcade55b77 --- /dev/null +++ b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h @@ -0,0 +1,198 @@ +/* + * 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. + */ + +#ifndef VP_PANDA3D_RENDERER_SET_H +#define VP_PANDA3D_RENDERER_SET_H + +#include + +#if defined(VISP_HAVE_PANDA3D) + +#include + +#include +#include + +BEGIN_VISP_NAMESPACE +/** + * \ingroup group_ar_renderer_panda3d + * + * @brief Class that rendering multiple datatypes, in a single pass. A RendererSet contains multiple subrenderers, all inheriting from vpPanda3DBaseRenderer. + * The renderer set synchronizes all scene properties for the different subrenderers. This includes: + * * The camera properties (intrinsics, resolution) and extrinsics + * * The pose and properties of every object in the scene + * * The pose and properties of lights, for subrenderers that are defined as lightable. + * + * The overall usage workflow is the following: + * 1. Create vpPanda3DRendererSet instance + * 2. Create the subrenderers (e.g, vpPanda3DGeometryRenderer) + * 3. Add the subrenderers to the set with addSubRenderer + * 4. Call renderFrame() on the rendererSet. Each subrenderer now has its output computed and ready to be retrieved + * 5. Retrieve relevant outputs in ViSP format with something similar to `rendererSet.getRenderer("MyRendererName").getRender(I)` where RendererType is the relevant subclass of vpPanda3DBaseRenderer and "MyRendererName" its name (see vpPanda3DBaseRenderer::getName) +*/ +class VISP_EXPORT vpPanda3DRendererSet : public vpPanda3DBaseRenderer, public vpPanda3DLightable +{ +public: + vpPanda3DRendererSet(const vpPanda3DRenderParameters &renderParameters); + + virtual ~vpPanda3DRendererSet() = default; + + /** + * @brief Initialize the framework and propagate the created panda3D framework to the subrenderers. + * + * The subrenderers will be initialized in the order of their priority as defined by vpPanda3DBaseRenderer::getRenderOrder + * Thus, if a renderer B depends on A for its render, and if B.getRenderOrder() > A.getRenderOrder() it can rely on A being initialized when B.initFromParent is called (along with the setupCamera, setupRenderTarget). + */ + void initFramework() vp_override; + + /** + * @brief Set the pose of the camera, using the ViSP convention. This change is propagated to all subrenderers + * + * @param wTc Pose of the camera + */ + void setCameraPose(const vpHomogeneousMatrix &wTc) vp_override; + + /** + * @brief Retrieve the pose of the camera. As this renderer contains multiple other renderers. + * + * \warning It is assumed that all the sub renderers are synchronized (i.e., the setCameraPose of this renderer was called before calling this method). + * Otherwise, you may get incoherent results. + * + * @return the pose of the camera using the ViSP convention + */ + vpHomogeneousMatrix getCameraPose() vp_override; + + /** + * @brief Set the pose of an object for all the subrenderers. The pose is specified using the ViSP convention + * This method may fail if a subrenderer does not have a node with the given name. + * + * \warning This method may fail if a subrenderer does not have a node with the given name. It is assumed that the scenes are synchronized + * + * @param name + * @param wTo + */ + void setNodePose(const std::string &name, const vpHomogeneousMatrix &wTo) vp_override; + + /** + * @brief This method is not supported for this renderer type. Use the std::string version + * + * \throws vpException, as this method is not supported + * @param object + * @param wTo + */ + void setNodePose(NodePath &object, const vpHomogeneousMatrix &wTo) vp_override; + + /** + * @brief Retrieve the pose of a scene node. The pose is in the world frame, using a ViSP convention. + * + * \see the base method + * + * \warning It is assumed that all the sub renderers are synchronized (i.e., the setNodePose of this renderer was called before calling this method). + * Otherwise, you may get incoherent results. + * @param name name of the node + * @return vpHomogeneousMatrix the pose of the node in the world frame + */ + vpHomogeneousMatrix getNodePose(const std::string &name) vp_override; + + /** + * @brief This method is not supported for this renderer type. Use the std::string version + * + * \throws vpException, as this method is not supported + * @param object + */ + vpHomogeneousMatrix getNodePose(NodePath &object) vp_override; + + /** + * \warning This method is not supported and will throw + */ + void addNodeToScene(const NodePath &object) vp_override; + + void setRenderParameters(const vpPanda3DRenderParameters ¶ms) vp_override; + + void addLight(const vpPanda3DLight &light) vp_override; + + /** + * @brief Add a new subrenderer: This subrenderer should have a unique name, not present in the set. + * + * \throws if the subrenderer's name is already present in the set. + * + * @param renderer the renderer to add + */ + void addSubRenderer(std::shared_ptr renderer); + + /** + * @brief Retrieve the first subrenderer with the specified template type. + * + * @tparam RendererType The type of the renderer to find + * @return std::shared_ptr Pointer to the first renderer match, nullptr if none is found. + */ + template + std::shared_ptr getRenderer() + { + for (std::shared_ptr &renderer: m_subRenderers) { + std::shared_ptr rendererCast = std::dynamic_pointer_cast(renderer); + if (rendererCast != nullptr) { + return rendererCast; + } + } + return nullptr; + } + /** + * @brief Retrieve the subrenderer with the specified template type and the given name. + * + * @param name the name of the subrenderer to find + * @tparam RendererType The type of the renderer to find + * @return std::shared_ptr Pointer to the renderer, nullptr if none is found. + */ + template + std::shared_ptr getRenderer(const std::string &name) + { + for (std::shared_ptr &renderer: m_subRenderers) { + if (renderer->getName() == name) { + std::shared_ptr rendererCast = std::dynamic_pointer_cast(renderer); + if (rendererCast != nullptr) { + return rendererCast; + } + } + } + return nullptr; + } + +protected: + void setupScene() vp_override { } + + void setupCamera() vp_override { } + +private: + std::vector> m_subRenderers; +}; +END_VISP_NAMESPACE +#endif +#endif diff --git a/modules/ar/include/visp3/ar/vpSimulator.h b/modules/ar/include/visp3/ar/vpSimulator.h index cf6604a11c..260bad9e9d 100644 --- a/modules/ar/include/visp3/ar/vpSimulator.h +++ b/modules/ar/include/visp3/ar/vpSimulator.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,14 +29,10 @@ * * Description: * Simulator based on Coin3d. - * - * Authors: - * Anthony Saunier - * -*****************************************************************************/ + */ -#ifndef vpSimulator_HH -#define vpSimulator_HH +#ifndef VP_SIMULATOR_H +#define VP_SIMULATOR_H /*! \file vpSimulator.h \brief Implementation of a simulator based on Coin3d (www.coin3d.org). diff --git a/modules/ar/include/visp3/ar/vpSimulatorException.h b/modules/ar/include/visp3/ar/vpSimulatorException.h index f494e16552..e2a545222b 100644 --- a/modules/ar/include/visp3/ar/vpSimulatorException.h +++ b/modules/ar/include/visp3/ar/vpSimulatorException.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief Error that can be emitted by the vpSimulator class and its derivatives */ -#ifndef _vpSimulatorException_h_ -#define _vpSimulatorException_h_ +#ifndef VP_SIMULATOR_EXCEPTION_H +#define VP_SIMULATOR_EXCEPTION_H #include #include diff --git a/modules/ar/include/visp3/ar/vpViewer.h b/modules/ar/include/visp3/ar/vpViewer.h index 4e76034a3f..84d168ef7c 100644 --- a/modules/ar/include/visp3/ar/vpViewer.h +++ b/modules/ar/include/visp3/ar/vpViewer.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Simulator based on Coin3d. - * -*****************************************************************************/ + */ /*! \file vpViewer.h @@ -45,8 +43,8 @@ */ -#ifndef vpViewer_HH -#define vpViewer_HH +#ifndef VP_VIEWER_H +#define VP_VIEWER_H #include diff --git a/modules/ar/src/coin-simulator/vpAR.cpp b/modules/ar/src/coin-simulator/vpAR.cpp index 9c7c85996d..a2dbafdb69 100644 --- a/modules/ar/src/coin-simulator/vpAR.cpp +++ b/modules/ar/src/coin-simulator/vpAR.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +30,7 @@ * Description: * Use to display an image behind the internal view of the simulator * used for augmented reality application - * -*****************************************************************************/ + */ /*! \file vpAR.cpp @@ -66,17 +64,16 @@ BEGIN_VISP_NAMESPACE /*! - Basic Destructor that calls the kill() method of the vpSimulator - class. + Basic Destructor that calls the kill() method of the vpSimulator class. */ vpAR::~vpAR() { kill(); } /*! - Initialisation of the internal view of the simulator. + Initialisation of the internal view of the simulator. - \param width : Width of the internal view. - \param height : Height of the internal view. - \param type : Type of background image ie gray scaled or color. + \param width : Width of the internal view. + \param height : Height of the internal view. + \param type : Type of background image ie gray scaled or color. */ void vpAR::initInternalViewer(unsigned int width, unsigned int height, vpImageType type) { @@ -99,9 +96,9 @@ void vpAR::initInternalViewer(unsigned int width, unsigned int height, vpImageTy } /*! - Set the background image and turn it to deal with the frame of OpenGL. + Set the background image and turn it to deal with the frame of OpenGL. - \param I : Gray scaled image for the background. + \param I : Gray scaled image for the background. */ // Grey pictures SetBackGroundImage void vpAR::setImage(vpImage &I) @@ -122,14 +119,13 @@ void vpAR::setImage(vpImage &I) } /*! - Set the background image and turn it to deal with the frame of OpenGL. + Set the background image and turn it to deal with the frame of OpenGL. - \param I : Color image for the background. + \param I : Color image for the background. */ // Color pictures SetBackGroundImage void vpAR::setImage(vpImage &I) { - if ((internal_width != I.getWidth()) || (internal_height != I.getHeight())) { vpERROR_TRACE("The image size is different from the view size "); throw(vpException(vpException::dimensionError), "The image size is different from the view size"); diff --git a/modules/ar/src/coin-simulator/vpSimulator.cpp b/modules/ar/src/coin-simulator/vpSimulator.cpp index a3d2da3227..157b736d4b 100644 --- a/modules/ar/src/coin-simulator/vpSimulator.cpp +++ b/modules/ar/src/coin-simulator/vpSimulator.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Simulator based on Coin3d. - * - * Authors: - * Anthony Saunier - * -*****************************************************************************/ + */ /*! \file vpSimulator.cpp \brief Implementation of a simulator based on Coin3d (www.coin3d.org). diff --git a/modules/ar/src/coin-simulator/vpViewer.cpp b/modules/ar/src/coin-simulator/vpViewer.cpp index c987ceb469..cfb7703866 100644 --- a/modules/ar/src/coin-simulator/vpViewer.cpp +++ b/modules/ar/src/coin-simulator/vpViewer.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,8 +29,7 @@ * * Description: * Simulator based on Coin3d. - * -*****************************************************************************/ + */ /*! \file vpViewer.cpp Viewer used by the simulator. Under Windows, the viewer is diff --git a/modules/ar/src/ogre-simulator/vpAROgre.cpp b/modules/ar/src/ogre-simulator/vpAROgre.cpp index e371043efa..199ed37052 100644 --- a/modules/ar/src/ogre-simulator/vpAROgre.cpp +++ b/modules/ar/src/ogre-simulator/vpAROgre.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Augmented Reality viewer using Ogre3D. - * - * Authors: - * Bertrand Delabarre - * -*****************************************************************************/ + */ /*! \file vpAROgre.cpp diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp new file mode 100644 index 0000000000..5fb8d5b285 --- /dev/null +++ b/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp @@ -0,0 +1,235 @@ +/* + * 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. + */ + +#include + +#if defined(VISP_HAVE_PANDA3D) + +#include + +#include "load_prc_file.h" +#include + +BEGIN_VISP_NAMESPACE +const vpHomogeneousMatrix vpPanda3DBaseRenderer::VISP_T_PANDA({ + 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, -1., 0.0, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0 +}); +const vpHomogeneousMatrix vpPanda3DBaseRenderer::PANDA_T_VISP(vpPanda3DBaseRenderer::VISP_T_PANDA.inverse()); + +void vpPanda3DBaseRenderer::initFramework() +{ + if (m_framework.use_count() > 0) { + 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 = m_framework->open_window(winProps, flags); + // try and reopen with visible window + if (m_window == nullptr) { + winProps.set_minimized(true); + m_window = m_framework->open_window(winProps, 0); + } + if (m_window == nullptr) { + throw vpException(vpException::notInitialized, + "Panda3D renderer: Could not create the requested window when performing initialization."); + } + m_window->set_background_type(WindowFramework::BackgroundType::BT_black); + setupScene(); + setupCamera(); + setupRenderTarget(); + //m_window->get_display_region_3d()->set_camera(m_cameraPath); +} + +void vpPanda3DBaseRenderer::initFromParent(std::shared_ptr framework, PT(WindowFramework) window) +{ + m_framework = framework; + m_window = window; + setupScene(); + setupCamera(); + setupRenderTarget(); +} + +void vpPanda3DBaseRenderer::setupScene() +{ + m_renderRoot = m_window->get_render().attach_new_node(m_name); + //m_renderRoot.set_antialias(AntialiasAttrib::M_none); +} + +void vpPanda3DBaseRenderer::setupCamera() +{ + m_cameraPath = m_window->make_camera(); + m_camera = (Camera *)m_cameraPath.node(); + // m_camera = m_window->get_camera(0); + m_cameraPath = m_renderRoot.attach_new_node(m_camera); + m_renderParameters.setupPandaCamera(m_camera); + m_camera->set_scene(m_renderRoot); +} + +void vpPanda3DBaseRenderer::renderFrame() +{ + m_framework->get_graphics_engine()->render_frame(); +} + +void vpPanda3DBaseRenderer::setCameraPose(const vpHomogeneousMatrix &wTc) +{ + if (m_camera.is_null() || m_cameraPath.is_empty()) { + throw vpException(vpException::notInitialized, "Camera was not initialized before trying to set its pose"); + } + setNodePose(m_cameraPath, wTc); +} + +vpHomogeneousMatrix vpPanda3DBaseRenderer::getCameraPose() +{ + if (m_camera.is_null()) { + throw vpException(vpException::notInitialized, "Camera was not initialized before trying to get its pose"); + } + return getNodePose(m_cameraPath); +} + +void vpPanda3DBaseRenderer::setNodePose(const std::string &name, const vpHomogeneousMatrix &wTo) +{ + NodePath object = m_renderRoot.find(name); + setNodePose(object, wTo); +} + +void vpPanda3DBaseRenderer::setNodePose(NodePath &object, const vpHomogeneousMatrix &wTo) +{ + const vpHomogeneousMatrix wpTo = wTo * VISP_T_PANDA; + vpTranslationVector t = wpTo.getTranslationVector(); + vpQuaternionVector q(wpTo.getRotationMatrix()); + object.set_pos(t[0], t[1], t[2]); + object.set_quat(LQuaternion(q.w(), q.x(), q.y(), q.z())); +} + +vpHomogeneousMatrix vpPanda3DBaseRenderer::getNodePose(const std::string &name) +{ + NodePath object = m_renderRoot.find(name); + if (object.is_empty()) { + throw vpException(vpException::badValue, "Node %s was not found", name.c_str()); + } + return getNodePose(object); +} + +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]); + const vpQuaternionVector q(quat.get_i(), quat.get_j(), quat.get_k(), quat.get_r()); + return vpHomogeneousMatrix(t, q) * PANDA_T_VISP; +} + +void vpPanda3DBaseRenderer::computeNearAndFarPlanesFromNode(const std::string &name, float &nearV, float &farV) +{ + if (m_camera == nullptr) { + throw vpException(vpException::notInitialized, "Cannot compute planes when the camera is not initialized"); + } + NodePath object = m_renderRoot.find(name); + 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()); +} + +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; + model.detach_node(); + model.set_name(nodeName); + return model; +} + +void vpPanda3DBaseRenderer::addNodeToScene(const NodePath &object) +{ + NodePath objectInScene = object.copy_to(m_renderRoot); + objectInScene.set_name(object.get_name()); + setNodePose(objectInScene, vpHomogeneousMatrix()); +} + +void vpPanda3DBaseRenderer::setVerticalSyncEnabled(bool useVsync) +{ + if (useVsync) { + load_prc_file_data("", "sync-video true"); + } + else { + load_prc_file_data("", "sync-video false"); + } +} +void vpPanda3DBaseRenderer::setAbortOnPandaError(bool abort) +{ + if (abort) { + load_prc_file_data("", "assert-abort 1"); + } + else { + load_prc_file_data("", "assert-abort 0"); + } +} + +void vpPanda3DBaseRenderer::enableDebugLog() +{ + load_prc_file_data("", "gl-debug 1"); + load_prc_file_data("", "notify-level-display spam"); +} + +vpColVector vpPanda3DBaseRenderer::vispPointToPanda(const vpColVector &point) +{ + vpColVector pandaPos = PANDA_T_VISP * point; + pandaPos /= pandaPos[3]; + return pandaPos; +} +vpColVector vpPanda3DBaseRenderer::vispVectorToPanda(const vpColVector &point) +{ + vpColVector pandaPos = PANDA_T_VISP.getRotationMatrix() * point; + return pandaPos; +} + +void vpPanda3DBaseRenderer::printStructure() +{ + m_renderRoot.ls(); +} + +END_VISP_NAMESPACE + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_ar.a(vpPanda3DBaseRenderer.cpp.o) has no symbols +void dummy_vpPanda3DBaseRenderer() { }; + +#endif diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DCommonFilters.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DCommonFilters.cpp new file mode 100644 index 0000000000..5ef57c4b48 --- /dev/null +++ b/modules/ar/src/panda3d-simulator/vpPanda3DCommonFilters.cpp @@ -0,0 +1,219 @@ +/* + * 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. + */ + +#include +#include + +#if defined(VISP_HAVE_PANDA3D) + +BEGIN_VISP_NAMESPACE +const char *vpPanda3DLuminanceFilter::FRAGMENT_SHADER = R"shader( +#version 330 + +in vec2 texcoords; + +uniform sampler2D p3d_Texture0; + +out vec4 p3d_FragData; + +void main() { + vec4 v = texture(p3d_Texture0, texcoords); + p3d_FragData.b = 0.299 * v.r + 0.587 * v.g + 0.114 * v.b; +} +)shader"; + +vpPanda3DLuminanceFilter::vpPanda3DLuminanceFilter(const std::string &name, std::shared_ptr inputRenderer, bool isOutput) + : vpPanda3DPostProcessFilter(name, inputRenderer, isOutput, std::string(vpPanda3DLuminanceFilter::FRAGMENT_SHADER)) +{ } +FrameBufferProperties vpPanda3DLuminanceFilter::getBufferProperties() const +{ + FrameBufferProperties fbp; + fbp.set_depth_bits(0); + fbp.set_rgba_bits(0, 0, 8, 0); + fbp.set_float_color(false); + return fbp; +} +void vpPanda3DLuminanceFilter::getRender(vpImage &I) const +{ + vpPanda3DPostProcessFilter::getRenderBasic(I); +} + +const char *vpPanda3DGaussianBlur::FRAGMENT_SHADER = R"shader( +#version 330 + +in vec2 texcoords; + +uniform sampler2D p3d_Texture0; +uniform vec2 dp; // 1 divided by number of pixels + +const float kernel[25] = float[25]( + 2, 4, 5, 4, 2, + 4, 9, 12, 9, 4, + 5, 12, 15, 12, 5, + 4, 9, 12, 9, 4, + 2, 4, 5, 4, 2 +); +const float normalize = 1 / 159.0; + +vec2 offset[25] = vec2[25]( + vec2(-2*dp.x,-2*dp.y), vec2(-dp.x,-2*dp.y), vec2(0,-2*dp.y), vec2(dp.x,-2*dp.y), vec2(2*dp.x,-2*dp.y), + vec2(-2*dp.x,-dp.y), vec2(-dp.x, -dp.y), vec2(0.0, -dp.y), vec2(dp.x, -dp.y), vec2(2*dp.x,-dp.y), + vec2(-2*dp.x,0.0), vec2(-dp.x, 0.0), vec2(0.0, 0.0), vec2(dp.x, 0.0), vec2(2*dp.x,0.0), + vec2(-2*dp.x, dp.y), vec2(-dp.x, dp.y), vec2(0.0, dp.y), vec2(dp.x, dp.y), vec2(2*dp.x, dp.y), + vec2(-2*dp.x, 2*dp.y), vec2(-dp.x, 2*dp.y), vec2(0.0, 2*dp.y), vec2(dp.x, 2*dp.y), vec2(2*dp.x, 2*dp.y) +); + +out vec4 p3d_FragData; + +void main() { + float v = 0.f; + + for(int i = 0; i < 25; ++i) { + v += kernel[i] * texture(p3d_Texture0, texcoords + offset[i]).b ; + } + p3d_FragData.b = v * normalize; +} +)shader"; + +vpPanda3DGaussianBlur::vpPanda3DGaussianBlur(const std::string &name, std::shared_ptr inputRenderer, bool isOutput) + : vpPanda3DPostProcessFilter(name, inputRenderer, isOutput, vpPanda3DGaussianBlur::FRAGMENT_SHADER) +{ } + +FrameBufferProperties vpPanda3DGaussianBlur::getBufferProperties() const +{ + FrameBufferProperties fbp; + fbp.set_depth_bits(0); + fbp.set_rgba_bits(0, 0, 8, 0); + fbp.set_float_color(false); + return fbp; +} + +void vpPanda3DGaussianBlur::getRender(vpImage &I) const +{ + vpPanda3DPostProcessFilter::getRenderBasic(I); +} + +const char *vpPanda3DCanny::FRAGMENT_SHADER = R"shader( +#version 330 + +in vec2 texcoords; + +uniform sampler2D p3d_Texture0; +uniform vec2 dp; // 1 divided by number of pixels +uniform float edgeThreshold; + +const float kernel[9] = float[9]( + 0.0, 1.0, 0.0, + 1.0,-4.0, 1.0, + 0.0, 1.0, 0.0 +); + +const float kernel_h[9] = float[9]( + -1.0, 0.0, 1.0, + -2.0, 0.0, 2.0, + -1.0, 0.0, 1.0 +); + +const float kernel_v[9] = float[9]( + -1.0, -2.0, -1.0, + 0.0, 0.0, 0.0, + 1.0, 2.0, 1.0 +); + +vec2 offset[9] = vec2[9]( + vec2(-dp.x, -dp.y), vec2(0.0, -dp.y), vec2(dp.x, -dp.y), + vec2(-dp.x, 0.0), vec2(0.0, 0.0), vec2(dp.x, 0.0), + vec2(-dp.x, dp.y), vec2(0.0, dp.y), vec2(dp.x, dp.y) +); + + +out vec4 p3d_FragData; + +void main() { + float sum = 0.f; + for(int i = 0; i < 9; ++i) { + float pix = texture(p3d_Texture0, texcoords + offset[i]).b; + sum += pix * kernel[i]; + } + if(abs(sum * 255.f) > edgeThreshold) { + float sum_h = 0.f; + float sum_v = 0.f; + for(int i = 0; i < 9; ++i) { + float pix = texture(p3d_Texture0, texcoords + offset[i]).b; + sum_h += pix * kernel_h[i]; + 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); + p3d_FragData = vec4(sum_h, sum_v, orientationAndValid.x, orientationAndValid.y); + } else { + p3d_FragData = vec4(0.f, 0.f, 0.f, 0.f); + } +} +)shader"; + +vpPanda3DCanny::vpPanda3DCanny(const std::string &name, std::shared_ptr inputRenderer, bool isOutput, float edgeThreshold) + : vpPanda3DPostProcessFilter(name, inputRenderer, isOutput, vpPanda3DCanny::FRAGMENT_SHADER), m_edgeThreshold(edgeThreshold) +{ } + +void vpPanda3DCanny::setupScene() +{ + vpPanda3DPostProcessFilter::setupScene(); + m_renderRoot.set_shader_input("edgeThreshold", LVector2f(m_edgeThreshold)); +} + +void vpPanda3DCanny::setEdgeThreshold(float edgeThreshold) +{ + m_edgeThreshold = edgeThreshold; + m_renderRoot.set_shader_input("edgeThreshold", LVector2f(m_edgeThreshold)); +} + + +FrameBufferProperties vpPanda3DCanny::getBufferProperties() const +{ + FrameBufferProperties fbp; + fbp.set_depth_bits(0); + fbp.set_rgba_bits(32, 32, 32, 32); + fbp.set_float_color(true); + return fbp; +} + +void vpPanda3DCanny::getRender(vpImage &I) const +{ + vpPanda3DPostProcessFilter::getRenderBasic(I); +} + +END_VISP_NAMESPACE + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_ar.a(vpPanda3DCanny.cpp.o) has no symbols +void dummy_vpPanda3DCanny() { }; + +#endif diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp new file mode 100644 index 0000000000..7700089823 --- /dev/null +++ b/modules/ar/src/panda3d-simulator/vpPanda3DGeometryRenderer.cpp @@ -0,0 +1,262 @@ +/* + * 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. + */ + +#include + +#if defined(VISP_HAVE_PANDA3D) + +#include + +BEGIN_VISP_NAMESPACE +const char *vpPanda3DGeometryRenderer::SHADER_VERT_NORMAL_AND_DEPTH_CAMERA = R"shader( +#version 330 + +in vec3 p3d_Normal; +in vec4 p3d_Vertex; + +out vec3 oNormal; +uniform mat3 p3d_NormalMatrix; +uniform mat4 p3d_ModelViewMatrix; +uniform mat4 p3d_ModelViewProjectionMatrix; + + +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; + distToCamera = -cs_position.z; +} +)shader"; + +const char *vpPanda3DGeometryRenderer::SHADER_VERT_NORMAL_AND_DEPTH_WORLD = R"shader( + +#version 330 +in vec3 p3d_Normal; +in vec4 p3d_Vertex; +uniform mat4 p3d_ModelViewMatrix; +uniform mat4 p3d_ModelViewProjectionMatrix; +out vec3 oNormal; +out float distToCamera; + + +void main() +{ + //gl_Position = ftransform(); + gl_Position = p3d_ModelViewProjectionMatrix * p3d_Vertex; + oNormal = normalize(p3d_Normal); + oNormal.yz = oNormal.zy; + oNormal.y = -oNormal.y; + vec4 cs_position = p3d_ModelViewMatrix * p3d_Vertex; + distToCamera = -cs_position.z; +} +)shader"; + +const char *vpPanda3DGeometryRenderer::SHADER_FRAG_NORMAL_AND_DEPTH = R"shader( +#version 330 + +in vec3 oNormal; +in float distToCamera; +out vec4 p3d_FragColor; +out vec4 fragColor; + +out vec4 p3d_FragData; + + +void main() +{ + vec3 n = normalize(oNormal); + //if (!gl_FrontFacing) + //n = -n; + p3d_FragData = vec4(n, distToCamera); + +} +)shader"; + +std::string renderTypeToName(vpPanda3DGeometryRenderer::vpRenderType type) +{ + switch (type) { + case vpPanda3DGeometryRenderer::vpRenderType::OBJECT_NORMALS: + return "normals-world"; + case vpPanda3DGeometryRenderer::vpRenderType::CAMERA_NORMALS: + return "normals-camera"; + default: + return ""; + } +} + +vpPanda3DGeometryRenderer::vpPanda3DGeometryRenderer(vpRenderType renderType) : vpPanda3DBaseRenderer(renderTypeToName(renderType)), m_renderType(renderType) { } + +void vpPanda3DGeometryRenderer::setupScene() +{ + m_renderRoot = m_window->get_render().attach_new_node(m_name); + PT(Shader) shader; + if (m_renderType == OBJECT_NORMALS) { + shader = Shader::make(Shader::ShaderLanguage::SL_GLSL, + SHADER_VERT_NORMAL_AND_DEPTH_WORLD, + 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); + } + m_renderRoot.set_shader(shader); +} + +void vpPanda3DGeometryRenderer::setupRenderTarget() +{ + if (m_window == nullptr) { + throw vpException(vpException::fatalError, "Cannot setup render target when window is null"); + } + FrameBufferProperties fbp; + fbp.set_rgb_color(true); + fbp.set_float_depth(false); + fbp.set_float_color(true); + fbp.set_depth_bits(16); + fbp.set_rgba_bits(32, 32, 32, 32); + + WindowProperties win_prop; + win_prop.set_size(m_renderParameters.getImageWidth(), m_renderParameters.getImageHeight()); + // Don't open a window - force it to be an offscreen buffer. + int flags = GraphicsPipe::BF_refuse_window | GraphicsPipe::BF_resizeable; + GraphicsOutput *windowOutput = m_window->get_graphics_output(); + GraphicsEngine *engine = windowOutput->get_engine(); + GraphicsPipe *pipe = windowOutput->get_pipe(); + + m_normalDepthBuffer = engine->make_output(pipe, renderTypeToName(m_renderType), -100, fbp, win_prop, flags, + windowOutput->get_gsg(), windowOutput); + + if (m_normalDepthBuffer == nullptr) { + throw vpException(vpException::fatalError, "Could not create geometry info buffer"); + } + // if (!m_normalDepthBuffer->is_valid()) { + // throw vpException(vpException::fatalError, "Geometry info buffer is invalid"); + // } + m_buffers.push_back(m_normalDepthBuffer); + m_normalDepthTexture = new Texture(); + 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->set_clear_color(LColor(0.f)); + m_normalDepthBuffer->set_clear_color_active(true); + DisplayRegion *region = m_normalDepthBuffer->make_display_region(); + if (region == nullptr) { + throw vpException(vpException::fatalError, "Could not create display region"); + } + region->set_camera(m_cameraPath); + region->set_clear_color(LColor(0.f)); +} + +void 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) { + throw vpException(vpException::badValue, "Unexpected data type in normals texture"); + } + + int rowIncrement = normals.getWidth() * 4; + 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]; + float *depthRow = depth[i]; + for (unsigned int j = 0; j < normals.getWidth(); ++j) { + normalRow[j].B = (data[j * 4]); + normalRow[j].G = (data[j * 4 + 1]); + normalRow[j].R = (data[j * 4 + 2]); + depthRow[j] = (data[j * 4 + 3]); + } + data += rowIncrement; + } +} + +void vpPanda3DGeometryRenderer::getRender(vpImage &normals) const +{ + normals.resize(m_normalDepthTexture->get_y_size(), m_normalDepthTexture->get_x_size()); + if (m_normalDepthTexture->get_component_type() != Texture::T_float) { + throw vpException(vpException::badValue, "Unexpected data type in normals texture"); + } + + int rowIncrement = normals.getWidth() * 4; + 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) { + normalRow[j].B = (data[j * 4]); + normalRow[j].G = (data[j * 4 + 1]); + normalRow[j].R = (data[j * 4 + 2]); + } + data += rowIncrement; + } +} + +void vpPanda3DGeometryRenderer::getRender(vpImage &depth) const +{ + depth.resize(m_normalDepthTexture->get_y_size(), m_normalDepthTexture->get_x_size()); + if (m_normalDepthTexture->get_component_type() != Texture::T_float) { + throw vpException(vpException::badValue, "Unexpected data type in normals texture"); + } + + int rowIncrement = depth.getWidth() * 4; + float *data = (float *)(&(m_normalDepthTexture->get_ram_image().front())); + data = data + rowIncrement * (depth.getHeight() - 1); + rowIncrement = -rowIncrement; + + for (unsigned int i = 0; i < depth.getHeight(); ++i) { + float *depthRow = depth[i]; + for (unsigned int j = 0; j < depth.getWidth(); ++j) { + depthRow[j] = (data[j * 4 + 3]); + } + data += rowIncrement; + } +} + +END_VISP_NAMESPACE + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_ar.a(vpPanda3DGeometryRenderer.cpp.o) has no symbols +void dummy_vpPanda3DGeometryRenderer() { }; + +#endif diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp new file mode 100644 index 0000000000..2daa1c9af8 --- /dev/null +++ b/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp @@ -0,0 +1,203 @@ +/* + * 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. + */ + +#include + +#if defined(VISP_HAVE_PANDA3D) + +#include + +BEGIN_VISP_NAMESPACE +const char *vpPanda3DPostProcessFilter::FILTER_VERTEX_SHADER = R"shader( +#version 330 +in vec4 p3d_Vertex; +uniform mat4 p3d_ModelViewProjectionMatrix; +in vec2 p3d_MultiTexCoord0; +out vec2 texcoords; + +void main() +{ + gl_Position = p3d_ModelViewProjectionMatrix * p3d_Vertex; + texcoords = p3d_MultiTexCoord0; +} +)shader"; + +void vpPanda3DPostProcessFilter::setupScene() +{ + CardMaker cm("cm"); + cm.set_frame_fullscreen_quad(); + m_renderRoot = NodePath(cm.generate()); // Render root is a 2D rectangle + m_renderRoot.set_depth_test(false); + m_renderRoot.set_depth_write(false); + GraphicsOutput *buffer = m_inputRenderer->getMainOutputBuffer(); + if (buffer == nullptr) { + throw vpException(vpException::fatalError, + "Cannot add a postprocess filter to a renderer that does not define getMainOutputBuffer()"); + } + m_shader = Shader::make(Shader::ShaderLanguage::SL_GLSL, + FILTER_VERTEX_SHADER, + 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()); +} + +void vpPanda3DPostProcessFilter::setupCamera() +{ + m_cameraPath = m_window->make_camera(); + m_camera = (Camera *)m_cameraPath.node(); + PT(OrthographicLens) lens = new OrthographicLens(); + lens->set_film_size(2, 2); + lens->set_film_offset(0, 0); + lens->set_near_far(-1000, 1000); + m_camera->set_lens(lens); + m_cameraPath = m_renderRoot.attach_new_node(m_camera); + m_camera->set_scene(m_renderRoot); +} + +void vpPanda3DPostProcessFilter::setupRenderTarget() +{ + + if (m_window == nullptr) { + throw vpException(vpException::fatalError, "Cannot setup render target when window is null"); + } + FrameBufferProperties fbp = getBufferProperties(); + WindowProperties win_prop; + win_prop.set_size(m_renderParameters.getImageWidth(), m_renderParameters.getImageHeight()); + + // Don't open a window - force it to be an offscreen buffer. + int flags = GraphicsPipe::BF_refuse_window | GraphicsPipe::BF_resizeable; + GraphicsOutput *windowOutput = m_window->get_graphics_output(); + GraphicsEngine *engine = windowOutput->get_engine(); + GraphicsStateGuardian *gsg = windowOutput->get_gsg(); + GraphicsPipe *pipe = windowOutput->get_pipe(); + m_buffer = engine->make_output(pipe, m_name, m_renderOrder, + fbp, win_prop, flags, + gsg, windowOutput); + if (m_buffer == nullptr) { + throw vpException(vpException::fatalError, "Could not create buffer"); + } + m_buffers.push_back(m_buffer); + //m_buffer->set_inverted(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->set_clear_color(LColor(0.f)); + m_buffer->set_clear_color_active(true); + DisplayRegion *region = m_buffer->make_display_region(); + if (region == nullptr) { + throw vpException(vpException::fatalError, "Could not create display region"); + } + region->set_camera(m_cameraPath); + region->set_clear_color(LColor(0.f)); +} + +void vpPanda3DPostProcessFilter::setRenderParameters(const vpPanda3DRenderParameters ¶ms) +{ + m_renderParameters = params; + unsigned int previousH = m_renderParameters.getImageHeight(), previousW = m_renderParameters.getImageWidth(); + bool resize = previousH != params.getImageHeight() || previousW != params.getImageWidth(); + + m_renderParameters = params; + if (m_window != nullptr) { + GraphicsOutput *buffer = m_inputRenderer->getMainOutputBuffer(); + m_renderRoot.set_shader_input("dp", LVector2f(1.0 / buffer->get_texture()->get_x_size(), 1.0 / buffer->get_texture()->get_y_size())); + } + 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()); + } + } + } +} + +void vpPanda3DPostProcessFilter::getRenderBasic(vpImage &I) const +{ + if (!m_isOutput) { + throw vpException(vpException::fatalError, "Tried to fetch output of a postprocessing filter that was configured as an intermediate output"); + } + + I.resize(m_renderParameters.getImageHeight(), m_renderParameters.getImageWidth()); + const unsigned numComponents = m_texture->get_num_components(); + int rowIncrement = I.getWidth() * numComponents; // we ask for only 8 bits image, but we may get an rgb image + unsigned char *data = (unsigned char *)(&(m_texture->get_ram_image().front())); + // Panda3D stores data upside down + data += rowIncrement * (I.getHeight() - 1); + 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]; + } + } +} + +void vpPanda3DPostProcessFilter::getRenderBasic(vpImage &I) const +{ + if (!m_isOutput) { + throw vpException(vpException::fatalError, "Tried to fetch output of a postprocessing filter that was configured as an intermediate output"); + } + + I.resize(m_renderParameters.getImageHeight(), m_renderParameters.getImageWidth()); + const unsigned numComponents = m_texture->get_num_components(); + int rowIncrement = I.getWidth() * numComponents; // we ask for only 8 bits image, but we may get an rgb image + float *data = (float *)(&(m_texture->get_ram_image().front())); + // Panda3D stores data upside down + data += rowIncrement * (I.getHeight() - 1); + 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]; + } + } +} + +END_VISP_NAMESPACE + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_ar.a(vpPanda3DPostProcessFilter.cpp.o) has no symbols +void dummy_vpPanda3DPostProcessFilter() { }; + +#endif diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp new file mode 100644 index 0000000000..ab6800fdaf --- /dev/null +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp @@ -0,0 +1,369 @@ +/* + * 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. + */ + +#include + +#if defined(VISP_HAVE_PANDA3D) + +#include "orthographicLens.h" +#include "cardMaker.h" +#include "texturePool.h" + +BEGIN_VISP_NAMESPACE +const char *vpPanda3DRGBRenderer::COOK_TORRANCE_VERT = R"shader( +#version 330 + +in vec3 p3d_Normal; +in vec4 p3d_Vertex; + +out vec3 oNormal; +out vec4 viewVertex; + +uniform mat3 p3d_NormalMatrix; +uniform mat4 p3d_ModelViewMatrix; +uniform mat4 p3d_ModelViewProjectionMatrix; + +in vec2 p3d_MultiTexCoord0; +out vec2 texcoords; +out vec3 F0; + +uniform struct p3d_MaterialParameters { + vec4 ambient; + vec4 diffuse; + vec4 emission; + vec3 specular; + float shininess; + + // These properties are new in 1.10. + vec4 baseColor; + float roughness; + float metallic; + float refractiveIndex; +} p3d_Material; + +vec3 computeF0(float ior, float metallic, vec3 baseColor) +{ + float F0f = pow(abs((1.0 - ior) / (1.0 + ior)), 2.0); + vec3 F0 = vec3(F0f, F0f, F0f); + return mix(F0, baseColor, metallic); +} + +void main() +{ + gl_Position = p3d_ModelViewProjectionMatrix * p3d_Vertex; + oNormal = p3d_NormalMatrix * normalize(p3d_Normal); + viewVertex = p3d_ModelViewMatrix * p3d_Vertex; + texcoords = p3d_MultiTexCoord0; + F0 = computeF0(p3d_Material.refractiveIndex, p3d_Material.metallic, p3d_Material.baseColor.xyz); +} +)shader"; + +const char *vpPanda3DRGBRenderer::COOK_TORRANCE_FRAG = R"shader( +// Version 330, specified when generating shader +#define M_PI 3.1415926535897932384626433832795 + +in vec3 oNormal; +in vec4 viewVertex; +in vec3 F0; + +out vec4 p3d_FragData; + +uniform struct { + vec4 ambient; +} p3d_LightModel; + +uniform struct p3d_LightSourceParameters { + // Primary light color. + vec4 color; + + // View-space position. If w=0, this is a directional light, with the xyz + // being -direction. + vec4 position; + + // constant, linear, quadratic attenuation in one vector + vec3 attenuation; + +} p3d_LightSource[4]; + +uniform struct p3d_MaterialParameters { + vec4 ambient; + vec4 diffuse; + vec4 emission; + vec3 specular; + float shininess; + + // These properties are new in 1.10. + vec4 baseColor; + float roughness; + float metallic; + float refractiveIndex; +} p3d_Material; + +in vec2 texcoords; +#ifdef HAS_TEXTURE +uniform sampler2D p3d_Texture0; +#endif + +float D(float roughness2, float hn) +{ + return (1.f / (M_PI * roughness2)) * pow(hn, (2.f / roughness2) - 2.f); +} + +float G(float hn, float nv, float nl, float vh) +{ + return min(1.0, min((2.f * hn * nv) / vh, (2.f * hn * nl) / vh)); +} + +vec3 F(vec3 F0, float vh) +{ + return F0 + (vec3(1.f, 1.f, 1.f) - F0) * pow(1.f - vh, 5); +} + +void main() +{ + vec3 n = normalize(oNormal); // normalized normal vector + vec3 v = normalize(-viewVertex.xyz); // normalized view vector + float nv = max(0.f, dot(n, v)); + float roughness2 = clamp(pow(p3d_Material.roughness, 2), 0.01, 0.99); + + #ifdef HAS_TEXTURE + vec4 baseColor = texture(p3d_Texture0, texcoords); + vec4 ambientColor = baseColor; + #else + vec4 ambientColor = p3d_Material.ambient; + vec4 baseColor = p3d_Material.baseColor; + #endif + + p3d_FragData = p3d_LightModel.ambient * baseColor; + + + for(int i = 0; i < p3d_LightSource.length(); ++i) { + + vec3 lf = p3d_LightSource[i].position.xyz - (viewVertex.xyz * p3d_LightSource[i].position.w); + float lightDist = length(lf); + vec3 l = normalize(lf); // normalized light vector + vec3 h = normalize(l + v); // half way vector + float hn = dot(h, n); + float nl = max(0.f, dot(n, l)); + float vh = max(0.f, dot(v, h)); + + vec3 aFac = p3d_LightSource[i].attenuation; + float attenuation = 1.f / (aFac[0] + aFac[1] * lightDist + aFac[2] * lightDist * lightDist); + + vec3 FV = F(F0, vh); + vec3 kd = (1.f - p3d_Material.metallic) * (1.f - FV) * (1.f / M_PI); + + + #ifdef SPECULAR + vec3 specularColor = vec3(0.f, 0.f, 0.f); + if(nl > 0.f && nv > 0.f) { + float DV = D(roughness2, hn); + float GV = G(hn, nv, nl, vh); + vec3 rs = (DV * GV * FV) / (4.f * nl * nv); + specularColor = rs * p3d_Material.specular; + } + #else + vec3 specularColor = vec3(0.0, 0.0, 0.0); + #endif + + p3d_FragData += (p3d_LightSource[i].color * attenuation) * nl * (baseColor * vec4(kd, 1.f) + vec4(specularColor, 1.f)); + } +} +)shader"; + +std::string vpPanda3DRGBRenderer::makeFragmentShader(bool hasTexture, bool specular) +{ + std::stringstream ss; + ss << "#version 330" << std::endl; + if (hasTexture) { + ss << "#define HAS_TEXTURE 1" << std::endl; + } + if (specular) { + ss << "#define SPECULAR 1" << std::endl; + } + else { + ss << "#undef SPECULAR" << std::endl; + } + ss << vpPanda3DRGBRenderer::COOK_TORRANCE_FRAG; + return ss.str(); +} + +void vpPanda3DRGBRenderer::addNodeToScene(const NodePath &object) +{ + NodePath objectInScene = object.copy_to(m_renderRoot); + objectInScene.set_name(object.get_name()); + TextureCollection txs = objectInScene.find_all_textures(); + bool hasTexture = txs.size() > 0; + // gltf2bam and other tools may store some fallback textures. We shouldnt use them as they whiten the result + if (hasTexture) { + std::vector fallbackNames { "pbr-fallback", "normal-fallback", "emission-fallback" }; + unsigned numMatches = 0; + for (const std::string &fallbackName: fallbackNames) { + numMatches += static_cast(txs.find_texture(fallbackName) != nullptr); + } + hasTexture = txs.size() > numMatches; // Some textures are not fallback textures + } + + PT(Shader) shader = Shader::make(Shader::ShaderLanguage::SL_GLSL, + COOK_TORRANCE_VERT, + makeFragmentShader(hasTexture, m_showSpeculars)); + + objectInScene.set_shader(shader); + + setNodePose(objectInScene, vpHomogeneousMatrix()); +} + +void vpPanda3DRGBRenderer::setBackgroundImage(const vpImage &background) +{ + + if (m_display2d == nullptr) { + CardMaker cm("card"); + cm.set_frame_fullscreen_quad(); + + NodePath myCamera2d(new Camera("myCam2d")); + PT(OrthographicLens) lens = new OrthographicLens(); + lens->set_film_size(2, 2); + lens->set_near_far(-1000, 1000); + lens->set_film_offset(0, 0); + ((Camera *)myCamera2d.node())->set_lens(lens); + + NodePath myRender2d("myRender2d"); + myRender2d.set_depth_test(false); + myRender2d.set_depth_write(false); + myCamera2d.reparent_to(myRender2d); + m_backgroundImage = myRender2d.attach_new_node(cm.generate()); + + m_display2d = m_colorBuffer->make_display_region(); + m_display2d->set_sort(-100); + m_display2d->set_camera(myCamera2d); + } + if (m_backgroundTexture == nullptr) { + m_backgroundTexture = new Texture(); + } + m_backgroundImage.set_texture(m_backgroundTexture); + m_backgroundTexture->setup_2d_texture(background.getWidth(), background.getHeight(), + Texture::ComponentType::T_unsigned_byte, + Texture::Format::F_rgba8); + //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; + for (unsigned int j = 0; j < background.getWidth(); ++j) { + destRow[j * 4] = srcRow[j].B; + destRow[j * 4 + 1] = srcRow[j].G; + destRow[j * 4 + 2] = srcRow[j].R; + destRow[j * 4 + 3] = srcRow[j].A; + } + } +} + +void vpPanda3DRGBRenderer::getRender(vpImage &I) const +{ + I.resize(m_colorTexture->get_y_size(), m_colorTexture->get_x_size()); + unsigned char *data = (unsigned char *)(&(m_colorTexture->get_ram_image().front())); + int rowIncrement = I.getWidth() * 4; + // Panda3D stores the image using the OpenGL convention (origin is bottom left), + // while we store data with origin as upper left. We copy with a flip + data = data + rowIncrement * (I.getHeight() - 1); + rowIncrement = -rowIncrement; + + 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]; + } + data += rowIncrement; + } +} + +void vpPanda3DRGBRenderer::setupScene() +{ + vpPanda3DBaseRenderer::setupScene(); + setLightableScene(m_renderRoot); +} + +void vpPanda3DRGBRenderer::setupRenderTarget() +{ + if (m_window == nullptr) { + throw vpException(vpException::fatalError, "Cannot setup render target when window is null"); + } + FrameBufferProperties fbp; + fbp.set_rgb_color(true); + fbp.set_float_depth(false); + fbp.set_float_color(false); + fbp.set_depth_bits(16); + fbp.set_rgba_bits(8, 8, 8, 8); + fbp.set_srgb_color(true); + + WindowProperties win_prop; + win_prop.set_size(m_renderParameters.getImageWidth(), m_renderParameters.getImageHeight()); + + // Don't open a window - force it to be an offscreen buffer. + int flags = GraphicsPipe::BF_refuse_window | GraphicsPipe::BF_resizeable; + GraphicsOutput *windowOutput = m_window->get_graphics_output(); + GraphicsEngine *engine = windowOutput->get_engine(); + GraphicsStateGuardian *gsg = windowOutput->get_gsg(); + GraphicsPipe *pipe = windowOutput->get_pipe(); + m_colorBuffer = engine->make_output(pipe, "Color Buffer", m_renderOrder, + fbp, win_prop, flags, + gsg, windowOutput); + if (m_colorBuffer == nullptr) { + throw vpException(vpException::fatalError, "Could not create color buffer"); + } + m_buffers.push_back(m_colorBuffer); + //m_colorBuffer->set_inverted(gsg->get_copy_texture_inverted()); + 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->set_clear_color(LColor(0.f)); + m_colorBuffer->set_clear_color_active(true); + DisplayRegion *region = m_colorBuffer->make_display_region(); + if (region == nullptr) { + throw vpException(vpException::fatalError, "Could not create display region"); + } + region->set_camera(m_cameraPath); + region->set_clear_color(LColor(0.f)); +} + +END_VISP_NAMESPACE + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_ar.a(vpPanda3DRGBRenderer.cpp.o) has no symbols +void dummy_vpPanda3DRGBRenderer() { }; + +#endif diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRenderParameters.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRenderParameters.cpp new file mode 100644 index 0000000000..43a4910359 --- /dev/null +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRenderParameters.cpp @@ -0,0 +1,74 @@ + +/* + * 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. + */ + +#include +#if defined(VISP_HAVE_PANDA3D) + +#include +#include + +BEGIN_VISP_NAMESPACE +void vpPanda3DRenderParameters::setupPandaCamera(Camera *camera) +{ + // Adapted from Megapose code (https://github.com/megapose6d/megapose6d/blob/master/src/megapose/panda3d_renderer/types.py#L59), + // which was itself inspired by https://discourse.panda3d.org/t/lens-camera-for-opencv-style-camera-parameterisation/15413 + // And http://ksimek.github.io/2013/06/03/calibrated_cameras_in_opengl + + if (m_width == 0 || m_height == 0) { + throw vpException(vpException::dimensionError, "Cannot create a projection matrix when the image width or height is 0"); + } + + PT(MatrixLens) lens = new MatrixLens(); + const double A = (m_clipFar + m_clipNear) / (m_clipFar - m_clipNear); + const double B = -2.0 * (m_clipFar * m_clipNear) / (m_clipFar - m_clipNear); + + const double cx = m_cam.get_u0(); + const double cy = m_height - m_cam.get_v0(); + + lens->set_near_far(m_clipNear, m_clipFar); + lens->set_user_mat(LMatrix4( + m_cam.get_px(), 0, 0, 0, + 0, 0, A, 1, + 0, m_cam.get_py(), 0, 0, + 0, 0, B, 0 + )); + lens->set_film_size(m_width, m_height); + lens->set_film_offset(m_width * 0.5 - cx, m_height * 0.5 - cy); + camera->set_lens(lens); +} + +END_VISP_NAMESPACE + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_ar.a(vpPanda3DRenderParameters.cpp.o) has no symbols +void dummy_vpPanda3DRenderParameters() { }; + +#endif diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp new file mode 100644 index 0000000000..3f1e0849a6 --- /dev/null +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp @@ -0,0 +1,188 @@ +/* + * 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. + */ + +#include + +#if defined(VISP_HAVE_PANDA3D) + +#include "load_prc_file.h" + +BEGIN_VISP_NAMESPACE +vpPanda3DRendererSet::vpPanda3DRendererSet(const vpPanda3DRenderParameters &renderParameters) : vpPanda3DBaseRenderer("set") +{ + m_renderParameters = renderParameters; + load_prc_file_data("", "textures-power-2 none"); +} + + +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"); + + + + if (m_framework.use_count() > 0) { + 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 = m_framework->open_window(winProps, flags); + if (m_window == nullptr) { + winProps.set_minimized(true); + m_window = m_framework->open_window(winProps, 0); + } + if (m_window == nullptr) { + throw vpException(vpException::fatalError, "Could not open Panda3D window (hidden or visible)"); + } + + m_window->set_background_type(WindowFramework::BackgroundType::BT_black); + for (std::shared_ptr &renderer: m_subRenderers) { + renderer->initFromParent(m_framework, m_window); + } +} + + +void vpPanda3DRendererSet::setCameraPose(const vpHomogeneousMatrix &wTc) +{ + for (std::shared_ptr &renderer: m_subRenderers) { + if (renderer->isRendering3DScene()) { + renderer->setCameraPose(wTc); + } + } +} + +vpHomogeneousMatrix vpPanda3DRendererSet::getCameraPose() +{ + if (m_subRenderers.size() == 0) { + throw vpException(vpException::fatalError, "cannot get the pose of an object if no subrenderer is present"); + } + if (!m_subRenderers[0]->isRendering3DScene()) { + throw vpException(vpException::fatalError, "This renderer set only contains a non-3D renderer."); + } + return m_subRenderers[0]->getCameraPose(); +} + +void vpPanda3DRendererSet::setNodePose(const std::string &name, const vpHomogeneousMatrix &wTo) +{ + for (std::shared_ptr &renderer: m_subRenderers) { + if (renderer->isRendering3DScene()) { + renderer->setNodePose(name, wTo); + } + } +} + +void vpPanda3DRendererSet::setNodePose(NodePath &object, const vpHomogeneousMatrix &wTo) +{ + throw vpException(vpException::badValue, "NodePath setNodePose is not supported in renderer set, prefer the string version"); +} + +vpHomogeneousMatrix vpPanda3DRendererSet::getNodePose(const std::string &name) +{ + if (m_subRenderers.size() == 0) { + throw vpException(vpException::fatalError, "cannot get the pose of an object if no subrenderer is present"); + } + if (!m_subRenderers[0]->isRendering3DScene()) { + throw vpException(vpException::fatalError, "This renderer set only contains a non-3D renderer."); + } + return m_subRenderers[0]->getNodePose(name); +} + +vpHomogeneousMatrix vpPanda3DRendererSet::getNodePose(NodePath &object) +{ + throw vpException(vpException::badValue, "NodePath getNodePose is not supported in renderer set, prefer the string version"); +} + +void vpPanda3DRendererSet::addNodeToScene(const NodePath &object) +{ + for (std::shared_ptr &renderer: m_subRenderers) { + if (renderer->isRendering3DScene()) { + renderer->addNodeToScene(object); + } + } +} + +void vpPanda3DRendererSet::setRenderParameters(const vpPanda3DRenderParameters ¶ms) +{ + m_renderParameters = params; + for (std::shared_ptr &renderer: m_subRenderers) { + renderer->setRenderParameters(m_renderParameters); + } +} + +void vpPanda3DRendererSet::addLight(const vpPanda3DLight &light) +{ + for (std::shared_ptr &renderer: m_subRenderers) { + vpPanda3DLightable *lightable = dynamic_cast(renderer.get()); + if (lightable != nullptr) { + lightable->addLight(light); + } + } +} + +void vpPanda3DRendererSet::addSubRenderer(std::shared_ptr renderer) +{ + for (std::shared_ptr &otherRenderer: m_subRenderers) { + if (renderer->getName() == otherRenderer->getName()) { + throw vpException(vpException::badValue, "Cannot have two subrenderers with the same name"); + } + } + std::vector>::const_iterator it = m_subRenderers.begin(); + while (it != m_subRenderers.end()) { + if ((*it)->getRenderOrder() > renderer->getRenderOrder()) { + break; + } + ++it; + } + m_subRenderers.insert(it, renderer); + for (const auto &r: m_subRenderers) { + std::cout << r->getName() << " "; + } + std::cout << std::endl; + + renderer->setRenderParameters(m_renderParameters); + if (m_framework != nullptr) { + renderer->initFromParent(m_framework, m_window); + renderer->setCameraPose(getCameraPose()); + } +} + +END_VISP_NAMESPACE + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_ar.a(vpPanda3DRendererSet.cpp.o) has no symbols +void dummy_vpPanda3DRendererSet() { }; + +#endif diff --git a/modules/ar/src/vpSimulatorException.cpp b/modules/ar/src/vpSimulatorException.cpp index a8049c364a..3c70f6d9fb 100644 --- a/modules/ar/src/vpSimulatorException.cpp +++ b/modules/ar/src/vpSimulatorException.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,13 +29,11 @@ * * Description: * Exceptions that can be emitted by the simulator classes. - * -*****************************************************************************/ + */ -/* \file vpSimulatorException.h +/*! \file vpSimulatorException.h \brief error that can be emitted by the vpSimulator class and its derivatives */ -/* Classes standards. */ #include diff --git a/tutorial/CMakeLists.txt b/tutorial/CMakeLists.txt index 468f5ffd7a..e80833ef2c 100644 --- a/tutorial/CMakeLists.txt +++ b/tutorial/CMakeLists.txt @@ -25,6 +25,7 @@ if(MSVC) endif() endif() +visp_add_subdirectory(ar REQUIRED_DEPS visp_core visp_gui visp_ar) visp_add_subdirectory(bridge/opencv REQUIRED_DEPS visp_core visp_io) visp_add_subdirectory(computer-vision REQUIRED_DEPS visp_core visp_blob visp_vision visp_io visp_gui visp_detection visp_sensor) visp_add_subdirectory(grabber REQUIRED_DEPS visp_core visp_sensor visp_io visp_gui) diff --git a/tutorial/ar/CMakeLists.txt b/tutorial/ar/CMakeLists.txt new file mode 100644 index 0000000000..d8591335f6 --- /dev/null +++ b/tutorial/ar/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.5) + +project(color_segmentation) + +find_package(VISP REQUIRED visp_core visp_gui visp_ar) + +# set the list of source files +set(tutorial_cpp + tutorial-panda3d-renderer.cpp +) + +visp_set_source_file_compile_flag(tutorial-panda3d-renderer.cpp -Wno-extra -Wno-unused-parameter -Wno-unused-variable) + +foreach(cpp ${tutorial_cpp}) + visp_add_target(${cpp}) + if(COMMAND visp_add_dependency) + visp_add_dependency(${cpp} "tutorials") + endif() +endforeach() + +# Copy the data folder to the same location than the target +visp_copy_dir(tutorial-panda3d-renderer "${CMAKE_CURRENT_SOURCE_DIR}" data) diff --git a/tutorial/ar/data/suzanne.bam b/tutorial/ar/data/suzanne.bam new file mode 100644 index 0000000000..79cf35730f Binary files /dev/null and b/tutorial/ar/data/suzanne.bam differ diff --git a/tutorial/ar/tutorial-panda3d-renderer.cpp b/tutorial/ar/tutorial-panda3d-renderer.cpp new file mode 100644 index 0000000000..84b357cf9c --- /dev/null +++ b/tutorial/ar/tutorial-panda3d-renderer.cpp @@ -0,0 +1,330 @@ +//! \example tutorial-panda3d-renderer.cpp +#include +#include + +#if defined(VISP_HAVE_PANDA3D) && defined(VISP_HAVE_DISPLAY) && defined(VISP_HAVE_MODULE_IO) + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +void displayNormals(const vpImage &normalsImage, + vpImage &normalDisplayImage) +{ +#if defined(_OPENMP) +#pragma omp parallel for +#endif + for (int i = 0; i < normalsImage.getSize(); ++i) { + normalDisplayImage.bitmap[i].R = static_cast((normalsImage.bitmap[i].R + 1.0) * 127.5f); + normalDisplayImage.bitmap[i].G = static_cast((normalsImage.bitmap[i].G + 1.0) * 127.5f); + normalDisplayImage.bitmap[i].B = static_cast((normalsImage.bitmap[i].B + 1.0) * 127.5f); + } + + vpDisplay::display(normalDisplayImage); + vpDisplay::flush(normalDisplayImage); +} + +void displayDepth(const vpImage &depthImage, + vpImage &depthDisplayImage, float nearV, float farV) +{ +#if defined(_OPENMP) +#pragma omp parallel for +#endif + for (int i = 0; i < depthImage.getSize(); ++i) { + float val = std::max(0.f, (depthImage.bitmap[i] - nearV) / (farV - nearV)); + depthDisplayImage.bitmap[i] = static_cast(val * 255.f); + } + vpDisplay::display(depthDisplayImage); + vpDisplay::flush(depthDisplayImage); +} + +void displayLightDifference(const vpImage &colorImage, const vpImage &colorDiffuseOnly, vpImage &lightDifference) +{ +#if defined(_OPENMP) +#pragma omp parallel for +#endif + for (int i = 0; i < colorImage.getSize(); ++i) { + float I1 = 0.299 * colorImage.bitmap[i].R + 0.587 * colorImage.bitmap[i].G + 0.114 * colorImage.bitmap[i].B; + float I2 = 0.299 * colorDiffuseOnly.bitmap[i].R + 0.587 * colorDiffuseOnly.bitmap[i].G + 0.114 * colorDiffuseOnly.bitmap[i].B; + lightDifference.bitmap[i] = static_cast(round(abs(I1 - I2))); + } + vpDisplay::display(lightDifference); + vpDisplay::flush(lightDifference); +} + +void displayCanny(const vpImage &cannyRawData, + vpImage &canny) +{ +#if defined(_OPENMP) +#pragma omp parallel for +#endif + for (int i = 0; i < cannyRawData.getSize(); ++i) { + vpRGBf &px = cannyRawData.bitmap[i]; + canny.bitmap[i] = 255 * (px.R * px.R + px.G * px.G > 0); + //canny.bitmap[i] = static_cast(127.5f + 127.5f * atan(px.B)); + } + + vpDisplay::display(canny); + for (unsigned int i = 0; i < canny.getHeight(); i += 8) { + for (unsigned int j = 0; j < canny.getWidth(); j += 8) { + bool valid = (pow(cannyRawData[i][j].R, 2.f) + pow(cannyRawData[i][j].G, 2.f)) > 0; + if (!valid) continue; + float angle = cannyRawData[i][j].B; + unsigned x = j + 10 * cos(angle); + unsigned y = i + 10 * sin(angle); + vpDisplay::displayArrow(canny, i, j, y, x, vpColor::green); + } + } + vpDisplay::flush(canny); +} + +int main(int argc, const char **argv) +{ + bool stepByStep = false; + bool debug = false; + bool showLightContrib = false; + bool showCanny = false; + char *modelPathCstr = 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, + "Show frames step by step."}, + {"-specular", vpParseArgv::ARGV_CONSTANT_BOOL, (char *) nullptr, (char *)&showLightContrib, + "Show frames step by step."}, + {"-canny", vpParseArgv::ARGV_CONSTANT_BOOL, (char *) nullptr, (char *)&showCanny, + "Show frames step by step."}, + {"-debug", vpParseArgv::ARGV_CONSTANT_BOOL, (char *) nullptr, (char *)&debug, + "Show Opengl/Panda3D debug message."}, + {"-h", vpParseArgv::ARGV_HELP, (char *) nullptr, (char *) nullptr, + "Print the help."}, + {(char *) nullptr, vpParseArgv::ARGV_END, (char *) nullptr, (char *) nullptr, (char *) nullptr} }; + + // Read the command line options + if (vpParseArgv::parse(&argc, argv, argTable, + vpParseArgv::ARGV_NO_LEFTOVERS | + vpParseArgv::ARGV_NO_ABBREV | + vpParseArgv::ARGV_NO_DEFAULTS)) { + return (false); + } + + std::string modelPath; + if (modelPathCstr) { + modelPath = modelPathCstr; + } + else { + modelPath = "data/suzanne.bam"; + } + std::string backgroundPath; + if (backgroundPathCstr) { + backgroundPath = backgroundPathCstr; + } + const std::string objectName = "object"; + + //! [Renderer set] + vpPanda3DRenderParameters renderParams(vpCameraParameters(300, 300, 160, 120), 240, 320, 0.01, 10.0); + vpPanda3DRendererSet renderer(renderParams); + renderer.setRenderParameters(renderParams); + renderer.setVerticalSyncEnabled(false); + renderer.setAbortOnPandaError(true); + if (debug) { + renderer.enableDebugLog(); + } + //! [Renderer set] + + //! [Subrenderers init] + std::shared_ptr geometryRenderer = std::make_shared(vpPanda3DGeometryRenderer::vpRenderType::OBJECT_NORMALS); + 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 blurFilter = std::make_shared("blur", grayscaleFilter, false); + std::shared_ptr cannyFilter = std::make_shared("canny", blurFilter, true, 10.f); + //! [Subrenderers init] + + //! [Adding subrenderers] + renderer.addSubRenderer(geometryRenderer); + renderer.addSubRenderer(cameraRenderer); + renderer.addSubRenderer(rgbRenderer); + if (showLightContrib) { + renderer.addSubRenderer(rgbDiffuseRenderer); + } + if (showCanny) { + renderer.addSubRenderer(grayscaleFilter); + renderer.addSubRenderer(blurFilter); + renderer.addSubRenderer(cannyFilter); + } + std::cout << "Initializing Panda3D rendering framework" << std::endl; + renderer.initFramework(); + //! [Adding subrenderers] + + //! [Scene configuration] + NodePath object = renderer.loadObject(objectName, modelPath); + renderer.addNodeToScene(object); + + vpPanda3DAmbientLight alight("Ambient", vpRGBf(0.2)); + renderer.addLight(alight); + + vpPanda3DPointLight plight("Point", vpRGBf(1.0), vpColVector({ 0.3, -0.4, -0.2 }), vpColVector({ 0.0, 0.0, 1.0 })); + renderer.addLight(plight); + + vpPanda3DDirectionalLight dlight("Directional", vpRGBf(2.0), vpColVector({ 1.0, 1.0, 0.0 })); + renderer.addLight(dlight); + + if (!backgroundPath.empty()) { + vpImage background; + vpImageIo::read(background, backgroundPath); + rgbRenderer->setBackgroundImage(background); + } + + rgbRenderer->printStructure(); + + 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; + vpImage cannyRawData; + vpImage depthImage; + vpImage colorImage(h, w); + vpImage colorDiffuseOnly(h, w); + vpImage lightDifference(h, w); + vpImage cannyImage(h, w); + + vpImage normalDisplayImage(h, w); + vpImage cameraNormalDisplayImage(h, w); + vpImage depthDisplayImage(h, w); + +#if defined(VISP_HAVE_GTK) + using DisplayCls = vpDisplayGTK; +#elif defined(VISP_HAVE_X11) + using DisplayCls = vpDisplayX; +#elif defined(HAVE_OPENCV_HIGHGUI) + using DisplayCls = vpDisplayOpenCV; +#elif defined(VISP_HAVE_GDI) + using DisplayCls = vpDisplayGDI; +#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"); + + DisplayCls dImageDiff; + if (showLightContrib) { + dImageDiff.init(lightDifference, w * 2 + 80, 0, "Specular/reflectance contribution"); + } + DisplayCls dCanny; + if (showCanny) { + dCanny.init(cannyImage, w * 2 + 80, h + 80, "Canny"); + } + renderer.renderFrame(); + bool end = false; + bool firstFrame = true; + std::vector renderTime, fetchTime, displayTime; + while (!end) { + float nearV = 0, farV = 0; + const double beforeComputeBB = vpTime::measureTimeMs(); + rgbRenderer->computeNearAndFarPlanesFromNode(objectName, nearV, farV); + renderParams.setClippingDistance(nearV, farV); + renderer.setRenderParameters(renderParams); + //std::cout << "Update clipping plane took " << vpTime::measureTimeMs() - beforeComputeBB << std::endl; + + const double beforeRender = vpTime::measureTimeMs(); + renderer.renderFrame(); + const double beforeFetch = vpTime::measureTimeMs(); + renderer.getRenderer(geometryRenderer->getName())->getRender(normalsImage, depthImage); + renderer.getRenderer(cameraRenderer->getName())->getRender(cameraNormalsImage); + renderer.getRenderer(rgbRenderer->getName())->getRender(colorImage); + if (showLightContrib) { + renderer.getRenderer(rgbDiffuseRenderer->getName())->getRender(colorDiffuseOnly); + } + if (showCanny) { + renderer.getRenderer()->getRender(cannyRawData); + } + + const double beforeConvert = vpTime::measureTimeMs(); + displayNormals(normalsImage, normalDisplayImage); + displayNormals(cameraNormalsImage, cameraNormalDisplayImage); + displayDepth(depthImage, depthDisplayImage, nearV, farV); + if (showLightContrib) { + displayLightDifference(colorImage, colorDiffuseOnly, lightDifference); + } + if (showCanny) { + displayCanny(cannyRawData, cannyImage); + } + + vpDisplay::display(colorImage); + vpDisplay::displayText(colorImage, 15, 15, "Click to quit", vpColor::red); + + if (stepByStep) { + vpDisplay::displayText(colorImage, 50, 15, "Next frame: space", vpColor::red); + } + if (vpDisplay::getClick(colorImage, false)) { + end = true; + } + vpDisplay::flush(colorImage); + const double endDisplay = vpTime::measureTimeMs(); + renderTime.push_back(beforeFetch - beforeRender); + fetchTime.push_back(beforeConvert - beforeFetch); + displayTime.push_back(endDisplay - beforeConvert); + std::string s; + if (stepByStep) { + bool next = false; + while (!next) { + vpDisplay::getKeyboardEvent(colorImage, s, true); + if (s == " ") { + next = true; + } + } + } + const double afterAll = vpTime::measureTimeMs(); + 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); + } + if (renderTime.size() > 0) { + std::cout << "Render time: " << vpMath::getMean(renderTime) << "ms +- " << vpMath::getStdev(renderTime) << "ms" << std::endl; + std::cout << "Panda3D -> vpImage time: " << vpMath::getMean(fetchTime) << "ms +- " << vpMath::getStdev(fetchTime) << "ms" << std::endl; + std::cout << "Display time: " << vpMath::getMean(displayTime) << "ms +- " << vpMath::getStdev(displayTime) << "ms" << std::endl; + } + return 0; +} + +#else + +int main() +{ + std::cerr << "Recompile ViSP with Panda3D as a third party to run this tutorial" << std::endl; + return EXIT_FAILURE; +} + +#endif diff --git a/tutorial/bridge/opencv/tutorial-bridge-opencv-camera-param.cpp b/tutorial/bridge/opencv/tutorial-bridge-opencv-camera-param.cpp index 33303df158..c5fb8621e7 100644 --- a/tutorial/bridge/opencv/tutorial-bridge-opencv-camera-param.cpp +++ b/tutorial/bridge/opencv/tutorial-bridge-opencv-camera-param.cpp @@ -15,7 +15,7 @@ int main() #ifdef ENABLE_VISP_NAMESPACE using namespace VISP_NAMESPACE_NAME; #endif -//! [Set ViSP camera parameters] + //! [Set ViSP camera parameters] double u0 = 326.6; double v0 = 215.0; double px = 582.7;