diff --git a/.vscode/settings.json b/.vscode/settings.json index 91290c4089..ea34b1c2fe 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -205,4 +205,7 @@ {"column": 120, "color": "#ffcc00"} ], "C_Cpp.default.compilerPath": "/usr/bin/g++", + "[json]": { + "editor.defaultFormatter": "vscode.json-language-features" + }, } diff --git a/doc/mainpage.dox.in b/doc/mainpage.dox.in index 87ecb769f8..c68d1f5959 100644 --- a/doc/mainpage.dox.in +++ b/doc/mainpage.dox.in @@ -526,6 +526,69 @@ in different ways. This will motivate us to continue the efforts. \defgroup group_mbt_xml_parser XML parsers XML parsers dedicated to model-based trackers. */ + + +/******************************************* + * Module rbt + *******************************************/ +/*! + \ingroup module_tracker + \defgroup module_rbt RBT: Render-Based Tracker module + Render-Based Tracker module +*/ + +/*! + \ingroup module_rbt + \defgroup group_rbt_core Core Render-Based Tracking functionalities + + This group contains the core classes that make Render-Based tracking work. The main interface for Render-Based tracking is vpRBTracker. +*/ + +/*! + \ingroup module_rbt + \defgroup group_rbt_trackers Trackable features + + These classes represent features that can be tracked by the render-based tracker. All trackable features should inherit from vpRBFeatureTracker + +*/ + +/*! + \ingroup module_rbt + \defgroup group_rbt_mask Object segmentation + + These classes allow to perform object segmentaiton from rendering information. This segmentation may be used downstream by feature trackers to filter features. + */ + +/*! + \ingroup module_rbt + \defgroup group_rbt_drift Drift and divergence detection + + These classes allow to detect tracking drift. +*/ + +/*! + \ingroup module_rbt + \defgroup group_rbt_rendering Rendering + + This group contains additional rendering utilities. +*/ + +/*! + \ingroup module_mbt + \defgroup group_mbt_features Features + Model-based trackers features. +*/ +/*! + \ingroup module_mbt + \defgroup group_mbt_faces Faces management + Faces management including visibility. +*/ +/*! + \ingroup module_mbt + \defgroup group_mbt_xml_parser XML parsers + XML parsers dedicated to model-based trackers. +*/ + /******************************************* * Module robot *******************************************/ diff --git a/doc/tutorial/rendering/tutorial-panda3d.dox b/doc/tutorial/rendering/tutorial-panda3d.dox index ce2e939217..9632a349bf 100644 --- a/doc/tutorial/rendering/tutorial-panda3d.dox +++ b/doc/tutorial/rendering/tutorial-panda3d.dox @@ -30,6 +30,14 @@ vpPanda3DBaseRenderer, which implements basic functions for a panda renderer. \section tutorial-panda3d-install Panda3D installation \subsection tutorial-panda3d-install-ubuntu Installation on Ubuntu +- It is recommended to install the assimp package before installing Panda3D. This will allow you to load .obj files, which are a common and easily exportable format. + Without it, you will be restricted to .bam files, which is a panda specific format. To convert to a .bam file, see \ref tutorial-panda3d-file-conversion. + + To install assimp, run: + \code{.sh} + $ sudo apt install libassimp-dev + \endcode + - 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 @@ -91,6 +99,14 @@ vpPanda3DBaseRenderer, which implements basic functions for a panda renderer. - Hereafter you will find the instructions to build Panda3D from source on macOS. + - It is recommended to install the assimp package before installing Panda3D. This will allow you to load .obj files, which are a common and easily exportable format. + Without it, you will be restricted to .bam files, which is a panda specific format. To convert to a .bam file, see \ref tutorial-panda3d-file-conversion. + + To install assimp, run: + \code{.sh} + $ brew install assimp + \endcode + - 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 @@ -114,7 +130,7 @@ vpPanda3DBaseRenderer, which implements basic functions for a panda renderer. \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) + $ python3 makepanda/makepanda.py --everything --installer --no-opencv --no-python --threads $(sysctl -n hw.logicalcpu) \endcode - At this point you can either @@ -166,6 +182,21 @@ vpPanda3DBaseRenderer, which implements basic functions for a panda renderer. - Installer are available for Windows browsing the [download](https://www.panda3d.org/download/) page. +\section tutorial-panda3d-file-conversion Converting a mesh to Panda's file format + +If you did not install assimp before installing Panda, you are fairly restricted in what mesh format you can load with Panda. + +Panda's recommended format is .bam, a binary format specific to the engine itself. +If you have Python, you can easily convert your model to a .bam file by doing the following: + +1. Installing the panda3D-gltf package: `pip install panda3d-gltf` +2. From your favorite 3D modelling software export your model to GLTF (available in blender) +3. Running the converter tool: + \code{.sh} + $ gltf2bam source.gltf output.bam + \endcode +4. If you encounter issues, check the tool's settings with `gltf2bam -h` + \section tutorial-panda3d-usage Using Panda3D for rendering An example that shows how to exploit Panda3D in ViSP to render a color image with support for textures and lighting, a diff --git a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h index 079e4e96a4..8b93212288 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h +++ b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h @@ -99,8 +99,10 @@ class VISP_EXPORT vpPanda3DPostProcessFilter : public vpPanda3DBaseRenderer PointerTo m_texture; PointerTo m_buffer; - static const char *FILTER_VERTEX_SHADER; + static const std::string 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 index cadb5c021e..9b872a2294 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h @@ -109,8 +109,8 @@ class VISP_EXPORT vpPanda3DRGBRenderer : public vpPanda3DBaseRenderer, public vp bool m_showSpeculars; PointerTo m_colorTexture; PointerTo m_colorBuffer; - static const char *COOK_TORRANCE_VERT; - static const char *COOK_TORRANCE_FRAG; + static const std::string COOK_TORRANCE_VERT; + static const std::string COOK_TORRANCE_FRAG; NodePath m_backgroundImage; PointerTo m_display2d; diff --git a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h index e3679e4468..4a3f142acf 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRendererSet.h @@ -44,7 +44,8 @@ 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. + * @brief Class that renders multiple datatypes, in a single pass. + * A renderer set 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 @@ -53,13 +54,20 @@ BEGIN_VISP_NAMESPACE * 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) + * 3. Add the subrenderers to the set with addSubRenderer() + * 4. Call renderFrame() on the renderer set. Each subrenderer now has its output computed and ready + * to be retrieved + * 5. Retrieve relevant outputs in ViSP format with something similar to + * \code + * rendererSet.getRenderer("MyRendererName").getRender(I) + * \endcode + * 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(); vpPanda3DRendererSet(const vpPanda3DRenderParameters &renderParameters); virtual ~vpPanda3DRendererSet() = default; diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp index cecd919ed0..39ee9cd9bc 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DPostProcessFilter.cpp @@ -61,8 +61,8 @@ void vpPanda3DPostProcessFilter::setupScene() "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); + 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())); m_renderRoot.set_texture(buffer->get_texture()); diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp index 954a644ceb..6330941acf 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRGBRenderer.cpp @@ -37,7 +37,7 @@ #include "texturePool.h" BEGIN_VISP_NAMESPACE -const char *vpPanda3DRGBRenderer::COOK_TORRANCE_VERT = +const std::string vpPanda3DRGBRenderer::COOK_TORRANCE_VERT = "#version 330\n" "in vec3 p3d_Normal;\n" "in vec4 p3d_Vertex;\n" @@ -76,7 +76,7 @@ const char *vpPanda3DRGBRenderer::COOK_TORRANCE_VERT = " F0 = computeF0(p3d_Material.refractiveIndex, p3d_Material.metallic, p3d_Material.baseColor.xyz);\n" "}\n"; -const char *vpPanda3DRGBRenderer::COOK_TORRANCE_FRAG = +const std::string vpPanda3DRGBRenderer::COOK_TORRANCE_FRAG = "// Version 330, specified when generating shader\n" "#define M_PI 3.1415926535897932384626433832795\n" "in vec3 oNormal;\n" @@ -200,8 +200,8 @@ void vpPanda3DRGBRenderer::addNodeToScene(const NodePath &object) } PT(Shader) shader = Shader::make(Shader::ShaderLanguage::SL_GLSL, - COOK_TORRANCE_VERT, - makeFragmentShader(hasTexture, m_showSpeculars)); + COOK_TORRANCE_VERT, + makeFragmentShader(hasTexture, m_showSpeculars)); objectInScene.set_shader(shader); @@ -245,12 +245,7 @@ void vpPanda3DRGBRenderer::setBackgroundImage(const vpImage &background) 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; - } + memcpy(destRow, srcRow, background.getWidth() * 4); } } diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp index 8051a7b4f3..e3c07389e9 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp @@ -35,6 +35,13 @@ #include "load_prc_file.h" BEGIN_VISP_NAMESPACE +vpPanda3DRendererSet::vpPanda3DRendererSet() : vpPanda3DBaseRenderer("set") +{ + load_prc_file_data("", "textures-power-2 none"); + load_prc_file_data("", "gl-version 3 2"); + load_prc_file_data("", "no-singular-invert"); +} + vpPanda3DRendererSet::vpPanda3DRendererSet(const vpPanda3DRenderParameters &renderParameters) : vpPanda3DBaseRenderer("set") { m_renderParameters = renderParameters; @@ -195,6 +202,8 @@ void vpPanda3DRendererSet::enableSharedDepthBuffer(vpPanda3DBaseRenderer &source } } +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() { }; diff --git a/modules/core/include/visp3/core/vpPlane.h b/modules/core/include/visp3/core/vpPlane.h index d57f03039a..df0a8ab5cb 100644 --- a/modules/core/include/visp3/core/vpPlane.h +++ b/modules/core/include/visp3/core/vpPlane.h @@ -48,10 +48,9 @@ BEGIN_VISP_NAMESPACE \brief This class defines the container for a plane geometrical structure. - A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where - (x,y,z) are the coordinates of a point and where \f$[A,B,C]^T\f$ is a normal + A plane is given by the equation \f$A*X + B*Y + C*Z + D = 0\f$ where + (X,Y,Z) are the coordinates of a point and where \f$[A,B,C]^T\f$ is the normal vector of the plane. - */ class VISP_EXPORT vpPlane { @@ -66,14 +65,15 @@ class VISP_EXPORT vpPlane vpPlane(); vpPlane(const vpPlane &P); vpPlane(double A, double B, double C, double D); - vpPlane(const vpPoint &P, const vpColVector &n, vpPlaneFrame frame = camera_frame); + vpPlane(const vpPoint &P, const vpColVector &normal, vpPlaneFrame frame = camera_frame); vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame = camera_frame); double computeZ(double x, double y) const; - void init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame = camera_frame); - void init(const vpColVector &P, const vpColVector &n); - void init(const vpPlane &P); + vpPlane &init(const vpPoint &P, const vpColVector &normal, vpPlaneFrame frame = camera_frame); + vpPlane &init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame = camera_frame); + vpPlane &init(const vpColVector &P, const vpColVector &n); + vpPlane &init(const vpPlane &P); // SET the parameter /*! Set plane parameter A. */ @@ -157,7 +157,7 @@ class VISP_EXPORT vpPlane friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPlane &p); // Operation with Plane - void projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const; + void projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj, vpPlaneFrame frame = camera_frame) const; double rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVector &H) const; diff --git a/modules/core/include/visp3/core/vpRGBa.h b/modules/core/include/visp3/core/vpRGBa.h index 2cba895692..d62a1002cf 100644 --- a/modules/core/include/visp3/core/vpRGBa.h +++ b/modules/core/include/visp3/core/vpRGBa.h @@ -45,6 +45,11 @@ #include #include +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) +#include +#endif + + BEGIN_VISP_NAMESPACE /*! \class vpRGBa @@ -123,7 +128,7 @@ class VISP_EXPORT vpRGBa /*! Copy constructor. */ - inline vpRGBa(const vpRGBa &v) : R(v.R), G(v.G), B(v.B), A(v.A) { } + inline vpRGBa(const vpRGBa &v) = default; /*! Create a RGBa value from a 4 dimension column vector. @@ -143,9 +148,9 @@ class VISP_EXPORT vpRGBa vpRGBa &operator=(const unsigned char &v); vpRGBa &operator=(const unsigned int &v); vpRGBa &operator=(const int &v); - vpRGBa &operator=(const vpRGBa &v); + vpRGBa &operator=(const vpRGBa &v) = default; #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - vpRGBa &operator=(const vpRGBa &&v); + vpRGBa &operator=(vpRGBa &&v) = default; #endif vpRGBa &operator=(const vpColVector &v); bool operator==(const vpRGBa &v) const; @@ -171,5 +176,11 @@ class VISP_EXPORT vpRGBa friend VISP_EXPORT vpRGBa operator*(const double &x, const vpRGBa &rgb); }; + +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) +static_assert(std::is_trivially_assignable_v); +static_assert(std::is_trivially_copyable_v); +#endif + END_VISP_NAMESPACE #endif diff --git a/modules/core/include/visp3/core/vpRGBf.h b/modules/core/include/visp3/core/vpRGBf.h index f420b16964..adb100b89a 100644 --- a/modules/core/include/visp3/core/vpRGBf.h +++ b/modules/core/include/visp3/core/vpRGBf.h @@ -43,6 +43,10 @@ #include #include +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) +#include +#endif + BEGIN_VISP_NAMESPACE /*! @@ -103,7 +107,7 @@ class VISP_EXPORT vpRGBf /*! Copy constructor. */ - inline vpRGBf(const vpRGBf &v) : R(v.R), G(v.G), B(v.B) { } + inline vpRGBf(const vpRGBf &v) = default; /*! Create a RGB value from a 3 dimensional column vector. @@ -116,9 +120,9 @@ class VISP_EXPORT vpRGBf vpRGBf &operator=(float v); vpRGBf &operator=(int v); - vpRGBf &operator=(const vpRGBf &v); + vpRGBf &operator=(const vpRGBf &v) = default; #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - vpRGBf &operator=(const vpRGBf &&v); + vpRGBf &operator=(vpRGBf &&v) = default; #endif vpRGBf &operator=(const vpColVector &v); bool operator==(const vpRGBf &v) const; @@ -144,5 +148,12 @@ class VISP_EXPORT vpRGBf friend VISP_EXPORT vpRGBf operator*(double x, const vpRGBf &rgb); friend VISP_EXPORT vpRGBf operator*(float x, const vpRGBf &rgb); }; + +#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) +static_assert(std::is_trivially_assignable_v); +static_assert(std::is_trivially_copyable_v); +#endif + END_VISP_NAMESPACE + #endif diff --git a/modules/core/src/image/vpRGBa.cpp b/modules/core/src/image/vpRGBa.cpp index 5a106ff9c0..24a10f8c9c 100644 --- a/modules/core/src/image/vpRGBa.cpp +++ b/modules/core/src/image/vpRGBa.cpp @@ -86,32 +86,6 @@ vpRGBa &vpRGBa::operator=(const int &v) return *this; } -/*! - Copy operator. -*/ -vpRGBa &vpRGBa::operator=(const vpRGBa &v) -{ - this->R = v.R; - this->G = v.G; - this->B = v.B; - this->A = v.A; - return *this; -} - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) -/*! - Move operator. -*/ -vpRGBa &vpRGBa::operator=(const vpRGBa &&v) -{ - this->R = std::move(v.R); - this->G = std::move(v.G); - this->B = std::move(v.B); - this->A = std::move(v.A); - return *this; -} -#endif - /*! Cast a vpColVector in a vpRGBa diff --git a/modules/core/src/image/vpRGBf.cpp b/modules/core/src/image/vpRGBf.cpp index b178397686..c722e6b9f2 100644 --- a/modules/core/src/image/vpRGBf.cpp +++ b/modules/core/src/image/vpRGBf.cpp @@ -69,30 +69,6 @@ vpRGBf &vpRGBf::operator=(int v) return *this; } -/*! - Copy operator. -*/ -vpRGBf &vpRGBf::operator=(const vpRGBf &v) -{ - this->R = v.R; - this->G = v.G; - this->B = v.B; - return *this; -} - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) -/*! - Move operator. -*/ -vpRGBf &vpRGBf::operator=(const vpRGBf &&v) -{ - this->R = std::move(v.R); - this->G = std::move(v.G); - this->B = std::move(v.B); - return *this; -} -#endif - /*! Cast a vpColVector in a vpRGBf diff --git a/modules/core/src/tools/geometry/vpPlane.cpp b/modules/core/src/tools/geometry/vpPlane.cpp index cf199673be..bbd5a8d32e 100644 --- a/modules/core/src/tools/geometry/vpPlane.cpp +++ b/modules/core/src/tools/geometry/vpPlane.cpp @@ -29,8 +29,7 @@ * * Description: * Plane geometrical structure. - * -*****************************************************************************/ + */ /*! \file vpPlane.cpp @@ -64,8 +63,8 @@ vpPlane::vpPlane() : A(0), B(0), C(0), D(0) { } /*! Plane constructor from A, B, C, D parameters. - A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where - (x,y,z) are the coordinates of a point and \f$[A,B,C]^T\f$ is the normal + A plane is given by the equation \f$A*X + B*Y + C*Z + D = 0\f$ where + (X,Y,Z) are the coordinates of a point and \f$[A,B,C]^T\f$ is the normal vector of the plane. \param a, b, c, d : Parameters of the plane. @@ -85,33 +84,34 @@ vpPlane::vpPlane(const vpPlane &P) : A(0), B(0), C(0), D(0) } /*! + Plane constructor from a point \e P on the plane and the \e normal to the plane. - Plane constructor from a point \e P on the plane and the normal - \e n to the plane. - - A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where - (x,y,z) are the coordinates of a point and \f$[A,B,C]^T\f$ is the normal + A plane is given by the equation \f$A*X + B*Y + C*Z + D = 0\f$ where + (X,Y,Z) are the coordinates of a point and \f$[A,B,C]^T\f$ is the normal vector of the plane. - \param P : A point with coordinates (x,y,z) on the plane. The \e frame - parameter indicates if the coordinates of this points that are used are - expressed in the camera of object frame. + \param P : A point with coordinates (X,Y,Z) on the plane. The \e frame + parameter indicates if the coordinates of this point are + expressed in the camera or object frame. - \param n : The normal to the plane. + \param normal : The normal to the plane. \param frame: Indicates if the plane should be initialized from the point P coordinates expressed in the camera or object frame. - + - When expressed in the camera frame we get the coordinates of the point using + (`P.get_X()`, `P.get_Y()`, `P.get_Z()`). + - When expressed in the object frame we get the coordinates of the point using + (`P.get_oX()`, `P.get_oY()`, `P.get_oZ()`). */ -vpPlane::vpPlane(const vpPoint &P, const vpColVector &n, vpPlaneFrame frame) : A(0), B(0), C(0), D(0) +vpPlane::vpPlane(const vpPoint &P, const vpColVector &normal, vpPlaneFrame frame) : A(0), B(0), C(0), D(0) { const unsigned int index_0 = 0; const unsigned int index_1 = 1; const unsigned int index_2 = 2; // Equation of the plane is given by: - A = n[index_0]; - B = n[index_1]; - C = n[index_2]; + A = normal[index_0]; + B = normal[index_1]; + C = normal[index_2]; if (frame == vpPlane::camera_frame) { D = -((A * P.get_X()) + (B * P.get_Y()) + (C * P.get_Z())); @@ -126,12 +126,52 @@ vpPlane::vpPlane(const vpPoint &P, const vpColVector &n, vpPlaneFrame frame) : A \param P : Plane used as initializer. */ -void vpPlane::init(const vpPlane &P) +vpPlane &vpPlane::init(const vpPlane &P) { setA(P.getA()); setB(P.getB()); setC(P.getC()); setD(P.getD()); + + return *this; +} + +/*! + Initialize the plane from a point \e P on the plane and the \e normal to the plane. + + \param P : A point with coordinates (X,Y,Z) on the plane. The \e frame + parameter indicates if the coordinates of this point are + expressed in the camera or object frame. + + \param normal : The normal to the plane. + + \param frame: Indicates if the plane should be initialized from the point P + coordinates expressed in the camera (X, Y, Z) or object frame (oX, oY, oZ). + - When expressed in the camera frame we get the coordinates of the point using + (`P.get_X()`, `P.get_Y()`, `P.get_Z()`). + - When expressed in the object frame we get the coordinates of the point using + (`P.get_oX()`, `P.get_oY()`, `P.get_oZ()`). + + \sa vpPlane(const vpPoint&, const vpColVector &) +*/ +vpPlane &vpPlane::init(const vpPoint &P, const vpColVector &normal, vpPlaneFrame frame) +{ + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + // Equation of the plane is given by: + A = normal[index_0]; + B = normal[index_1]; + C = normal[index_2]; + + if (frame == vpPlane::camera_frame) { + D = -((A * P.get_X()) + (B * P.get_Y()) + (C * P.get_Z())); + } + else { + D = -((A * P.get_oX()) + (B * P.get_oY()) + (C * P.get_oZ())); + } + + return *this; } /*! @@ -141,21 +181,23 @@ void vpPlane::init(const vpPlane &P) \param P : A point with coordinates (x,y,z) on the plane. The size of the vector should be 3, with P[0]=x, with P[1]=y, with P[2]=z. - \param n : The normal to the plane. + \param normal : The normal to the plane. \sa vpPlane(const vpPoint&, const vpColVector &) */ -void vpPlane::init(const vpColVector &P, const vpColVector &n) +vpPlane &vpPlane::init(const vpColVector &P, const vpColVector &normal) { const unsigned int index_0 = 0; const unsigned int index_1 = 1; const unsigned int index_2 = 2; // Equation of the plane is given by: - A = n[index_0]; - B = n[index_1]; - C = n[index_2]; + A = normal[index_0]; + B = normal[index_1]; + C = normal[index_2]; D = -((A * P[0]) + (B * P[1]) + (C * P[index_2])); + + return *this; } /*! @@ -169,7 +211,7 @@ void vpPlane::init(const vpColVector &P, const vpColVector &n) coordinates expressed in the camera or object frame. */ -void vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame) +vpPlane &vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame) { vpColVector a(3); vpColVector b(3); @@ -218,6 +260,8 @@ void vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlane B /= norm; C /= norm; D /= norm; + + return *this; } /*! @@ -250,7 +294,7 @@ double vpPlane::computeZ(double x, double y) const /*! Return the normal to the plane. - A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where + A plane is given by the equation \f$A*X + B*Y + C*Z + D = 0\f$ where (x,y,z) is a point of R^3 and (A,B,C) are the coordinates of the normal. \sa getNormal(vpColVector &n) @@ -272,8 +316,8 @@ vpColVector vpPlane::getNormal() const /*! Return the normal to the plane. - A plane is given by the equation \f$Ax + By + Cz + D = 0\f$ where - (x,y,z) are the coordinates of a point and \f$[A,B,C]^T\f$ is normal + A plane is given by the equation \f$A*X + B*Y + C*Z + D = 0\f$ where + (X,Y,Z) are the coordinates of a point and \f$[A,B,C]^T\f$ is the normal vector of the plane. \sa getNormal() @@ -294,24 +338,43 @@ void vpPlane::getNormal(vpColVector &n) const /*! Compute the coordinates of the projection of a point on the plane. - \param P : point to be projected on the plane - \param Pproj : result of the projection (pproj belongs to the plane) + \param[in] P : Point to be projected on the plane. + \param[out] Pproj : Projected point. + \param[in] frame : Indicates if the point P coordinates are expressed in the camera or object frame. + - When expressed in the camera frame we get the coordinates of the point using + (`P.get_X()`, `P.get_Y()`, `P.get_Z()`). + - When expressed in the object frame we get the coordinates of the point using + (`P.get_oX()`, `P.get_oY()`, `P.get_oZ()`). */ -void vpPlane::projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const +void vpPlane::projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj, vpPlaneFrame frame) const { double x0, y0, z0; double rho; - x0 = P.get_X() / P.get_W(); - y0 = P.get_Y() / P.get_W(); - z0 = P.get_Z() / P.get_W(); + if (frame == vpPlane::camera_frame) { + x0 = P.get_X() / P.get_W(); + y0 = P.get_Y() / P.get_W(); + z0 = P.get_Z() / P.get_W(); - rho = -((A * x0) + (B * y0) + (C * z0) + D) / ((A * A) + (B * B) + (C * C)); + rho = -((A * x0) + (B * y0) + (C * z0) + D) / ((A * A) + (B * B) + (C * C)); - Pproj.set_X(x0 + (A * rho)); - Pproj.set_Y(y0 + (B * rho)); - Pproj.set_Z(z0 + (C * rho)); - Pproj.set_W(1); + Pproj.set_X(x0 + (A * rho)); + Pproj.set_Y(y0 + (B * rho)); + Pproj.set_Z(z0 + (C * rho)); + Pproj.set_W(1); + } + else { + x0 = P.get_oX() / P.get_oW(); + y0 = P.get_oY() / P.get_oW(); + z0 = P.get_oZ() / P.get_oW(); + + rho = -((A * x0) + (B * y0) + (C * z0) + D) / ((A * A) + (B * B) + (C * C)); + + Pproj.set_oX(x0 + (A * rho)); + Pproj.set_oY(y0 + (B * rho)); + Pproj.set_oZ(z0 + (C * rho)); + Pproj.set_oW(1); + } } double vpPlane::rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVector &H) const diff --git a/modules/java/android_sdk/libcxx_helper/CMakeLists.txt b/modules/java/android_sdk/libcxx_helper/CMakeLists.txt index caec127adc..2b4c404fc3 100644 --- a/modules/java/android_sdk/libcxx_helper/CMakeLists.txt +++ b/modules/java/android_sdk/libcxx_helper/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.6) +cmake_minimum_required(VERSION 3.10) # dummy target to bring libc++_shared.so into packages add_library(visp_jni_shared STATIC dummy.cpp) diff --git a/modules/python/bindings/CMakeLists.txt b/modules/python/bindings/CMakeLists.txt index 9f62524a58..370c9883b0 100644 --- a/modules/python/bindings/CMakeLists.txt +++ b/modules/python/bindings/CMakeLists.txt @@ -43,10 +43,33 @@ pybind11_add_module(_visp ${python_bindings_cpp_src}) # when installing the python module, pip will look into this subfolder for .so files to copy into the site-packages file(MAKE_DIRECTORY "${bindings_gen_location}/src") +get_target_property(PYTHON_CXX_FLAGS _visp COMPILE_OPTIONS) +get_target_property( PYTHON_LINKER_FLAGS _visp LINK_OPTIONS) + +if(PYTHON_CXX_FLAGS STREQUAL "PYTHON_CXX_FLAGS-NOTFOUND") + SET(PYTHON_CXX_FLAGS "") # Set to empty string +else() + SET(PYTHON_CXX_FLAGS "${PYTHON_CXX_FLAGS}") +endif() +if(PYTHON_LINKER_FLAGS STREQUAL "PYTHON_LINKER_FLAGS-NOTFOUND") + SET(PYTHON_LINKER_FLAGS "") # Set to empty string +else() + SET(PYTHON_LINKER_FLAGS "${PYTHON_LINKER_FLAGS}") +endif() +CHECK_CXX_COMPILER_FLAG("-flto=auto" COMPILER_SUPPORTS_FLTOAUTO) +if(COMPILER_SUPPORTS_FLTOAUTO) + SET(PYTHON_CXX_FLAGS "${PYTHON_CXX_FLAGS} -flto=auto") + SET(PYTHON_LINKER_FLAGS "${PYTHON_LINKER_FLAGS} -flto=auto") +endif() + +if(NOT PYTHON_CXX_FLAGS STREQUAL "") + set_target_properties(_visp PROPERTIES COMPILE_FLAGS ${PYTHON_CXX_FLAGS}) + set_target_properties(_visp PROPERTIES LINK_FLAGS ${PYTHON_LINKER_FLAGS}) +endif() + set_target_properties(_visp PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - ) # With MSVC, the compiled pyd file is placed in a Release/Debug folder set(build_configs "NONE" "RELEASE" "DEBUG" "RELEASEWITHDEBINFO" "RELWITHDEBINFO") diff --git a/modules/python/bindings/include/core.hpp b/modules/python/bindings/include/core.hpp index 67963a3619..6b8fe10c7a 100644 --- a/modules/python/bindings/include/core.hpp +++ b/modules/python/bindings/include/core.hpp @@ -39,6 +39,8 @@ #include "core/images.hpp" #include "core/pixel_meter.hpp" #include "core/image_conversions.hpp" +#include "core/display.hpp" + #endif diff --git a/modules/python/bindings/include/core/arrays.hpp b/modules/python/bindings/include/core/arrays.hpp index 44c4b38603..9280c199c0 100644 --- a/modules/python/bindings/include/core/arrays.hpp +++ b/modules/python/bindings/include/core/arrays.hpp @@ -206,6 +206,197 @@ void define_get_item_1d_array(PyClass &pyClass) }, py::keep_alive<0, 1>()); } + + + + +template +void define_set_item_1d_array(PyClass &pyClass) +{ + pyClass.def("__setitem__", [](Class &self, int i, const T value) { + const int rows = (int)self.getRows(); + if (i >= rows || i < -rows) { + std::stringstream ss; + ss << "Invalid indexing into a 2D array: got indices (" << i << ", :)" + << " but image has dimensions " << shape_to_string({ rows, self.getCols() }); + throw std::runtime_error(ss.str()); + } + if (i < 0) { + i = rows + i; + } + self[i] = value; + }); + pyClass.def("__setitem__", [](Class &self, py::slice slice, const T value) { + int rowStart, rowEnd, rowStep; + std::tie(rowStart, rowEnd, rowStep, std::ignore) = solveSliceIndices(slice, self.getRows()); + for (int i = rowStart; i < rowEnd; i += rowStep) { + self[i] = value; + } + }); + + pyClass.def("__setitem__", [](Class &self, py::slice sliceRows, py::array_t &values) { + int rowStart, rowEnd, rowStep, numRows; + std::tie(rowStart, rowEnd, rowStep, numRows) = solveSliceIndices(sliceRows, self.getRows()); + + py::buffer_info valuesInfo = values.request(); + + // Copy the array into each row (same values in each row) + if (valuesInfo.ndim == 1) { + + if (valuesInfo.shape[0] != numRows) { + throw std::runtime_error("Number of indexed elements and numpy array size do not match"); + } + const T *value_ptr = static_cast(valuesInfo.ptr); + + unsigned int k = 0; + for (int i = rowStart; i < rowEnd; i += rowStep) { + self[i] = value_ptr[k++]; + } + } + else { + throw std::runtime_error("Cannot write into 1D raw type array with multidimensional NumPy array"); + } + }); +} + +/* + * Image 2D indexing + */ +template +void define_set_item_2d_array(PyClass &pyClass) +{ + pyClass.def("__setitem__", [](Class &self, std::pair pair, const T &value) { + int i = pair.first, j = pair.second; + const int rows = (int)self.getRows(), cols = (int)self.getCols(); + if (i >= rows || j >= cols || i < -rows || j < -cols) { + std::stringstream ss; + ss << "Invalid indexing into a 2D array: got indices " << shape_to_string({ i, j }) + << " but image has dimensions " << shape_to_string({ rows, cols }); + throw std::runtime_error(ss.str()); + } + if (i < 0) { + i = rows + i; + } + if (j < 0) { + j = cols + j; + } + self[i][j] = value; + }); + pyClass.def("__setitem__", [](Class &self, int i, const T &value) { + const int rows = (int)self.getRows(); + if (i >= rows || i < -rows) { + std::stringstream ss; + ss << "Invalid indexing into a 2D array: got indices (" << i << ", :)" + << " but image has dimensions " << shape_to_string({ rows, self.getCols() }); + throw std::runtime_error(ss.str()); + } + if (i < 0) { + i = rows + i; + } + T *row = self[i]; + for (unsigned int j = 0; j < self.getCols(); ++j) { + row[j] = value; + } + }); + pyClass.def("__setitem__", [](Class &self, py::slice slice, const T &value) { + int rowStart, rowEnd, rowStep; + std::tie(rowStart, rowEnd, rowStep, std::ignore) = solveSliceIndices(slice, self.getRows()); + for (int i = rowStart; i < rowEnd; i += rowStep) { + T *row = self[i]; + for (unsigned int j = 0; j < self.getCols(); ++j) { + row[j] = value; + } + } + }); + pyClass.def("__setitem__", [](Class &self, std::tuple slices, const T &value) { + py::slice sliceRows, sliceCols; + int rowStart, rowEnd, rowStep; + int colStart, colEnd, colStep; + std::tie(sliceRows, sliceCols) = slices; + std::tie(rowStart, rowEnd, rowStep, std::ignore) = solveSliceIndices(sliceRows, self.getRows()); + std::tie(colStart, colEnd, colStep, std::ignore) = solveSliceIndices(sliceCols, self.getCols()); + + for (int i = rowStart; i < rowEnd; i += rowStep) { + T *row = self[i]; + for (int j = colStart; j < colEnd; j += colStep) { + row[j] = value; + } + } + }); + + + pyClass.def("__setitem__", [](Class &self, int row, py::array_t &values) { + if (row < 0) { + row = self.getRows() + row; + } + + if (row > static_cast(self.getRows())) { + throw std::runtime_error("Invalid row index when assigning to image"); + } + + // Copy the array into each row (same values in each row) + py::buffer_info valuesInfo = values.request(); + if (valuesInfo.ndim == 1) { + if (valuesInfo.shape[0] != self.getCols()) { + throw std::runtime_error("Number of image columns and NumPy array dimension do not match"); + } + + const T *value_ptr = static_cast(valuesInfo.ptr); + + T *row_ptr = self[row]; + for (unsigned int j = 0; j < self.getCols(); ++j) { + row_ptr[j] = value_ptr[j]; + } + } + else { + throw std::runtime_error("Cannot write into image row with a multidimensional array"); + } + }); + pyClass.def("__setitem__", [](Class &self, py::slice sliceRows, py::array_t &values) { + int rowStart, rowEnd, rowStep, numRows; + std::tie(rowStart, rowEnd, rowStep, numRows) = solveSliceIndices(sliceRows, self.getRows()); + + py::buffer_info valuesInfo = values.request(); + + // Copy the array into each row (same values in each row) + if (valuesInfo.ndim == 1) { + if (valuesInfo.shape[0] != self.getCols()) { + throw std::runtime_error("Number of image columns and NumPy array dimension do not match"); + } + + const T *value_ptr = static_cast(valuesInfo.ptr); + + for (int i = rowStart; i < rowEnd; i += rowStep) { + T *row = self[i]; + unsigned int k = 0; + for (unsigned int j = 0; j < self.getCols(); ++j) { + row[j] = value_ptr[k++]; + } + } + } + // 2D array to 2D array + else if (valuesInfo.ndim == 2) { + if (valuesInfo.shape[0] != numRows || valuesInfo.shape[1] != self.getCols()) { + throw std::runtime_error("Indexing into 2D image: NumPy array has wrong size"); + } + const T *value_ptr = static_cast(valuesInfo.ptr); + + unsigned int k = 0; + for (int i = rowStart; i < rowEnd; i += rowStep) { + T *row = self[i]; + + for (unsigned int j = 0; j < self.getCols(); j++) { + row[j] = value_ptr[k++]; + } + } + } + else { + throw std::runtime_error("Cannot write into 2D raw type image with multidimensional NumPy array that has more than 2 dimensions"); + } + }); +} + + const char *numpy_fn_doc_writable = R"doc( Numpy view of the underlying array data. This numpy view can be used to directly modify the array. @@ -240,6 +431,8 @@ Construct a 2D ViSP array by **copying** a 2D numpy array. )doc", py::arg("np_array")); define_get_item_2d_array, std::shared_ptr>>, vpArray2D, T>(pyArray2D); + define_set_item_2d_array, std::shared_ptr>>, vpArray2D, T>(pyArray2D); + } void bindings_vpMatrix(py::class_, vpArray2D> &pyMatrix) @@ -269,6 +462,7 @@ Construct a matrix by **copying** a 2D numpy array. add_cpp_print_helper(pyMatrix, &vpMatrix::cppPrint); define_get_item_2d_array, vpArray2D>, vpMatrix, double>(pyMatrix); + define_set_item_2d_array, vpArray2D>, vpMatrix, double>(pyMatrix); } @@ -350,6 +544,7 @@ Construct a Translation vector by **copying** a 1D numpy array of size 3. )doc", py::arg("np_array")); define_get_item_1d_array, vpArray2D>, vpTranslationVector, double>(pyTranslationVector); + define_set_item_1d_array, vpArray2D>, vpTranslationVector, double>(pyTranslationVector); } @@ -374,6 +569,7 @@ Construct a column vector by **copying** a 1D numpy array. )doc", py::arg("np_array")); define_get_item_1d_array, vpArray2D>, vpColVector, double>(pyColVector); + define_set_item_1d_array, vpArray2D>, vpColVector, double>(pyColVector); add_print_helper(pyColVector, &vpColVector::csvPrint, "strCsv", csv_str_help); add_print_helper(pyColVector, &vpColVector::maplePrint, "strMaple", maple_str_help); @@ -401,6 +597,8 @@ Construct a row vector by **copying** a 1D numpy array. )doc", py::arg("np_array")); define_get_item_1d_array, vpArray2D>, vpRowVector, double>(pyRowVector); + define_set_item_1d_array, vpArray2D>, vpRowVector, double>(pyRowVector); + add_print_helper(pyRowVector, &vpRowVector::csvPrint, "strCsv", csv_str_help); add_print_helper(pyRowVector, &vpRowVector::maplePrint, "strMaple", maple_str_help); add_print_helper(pyRowVector, &vpRowVector::matlabPrint, "strMatlab", matlab_str_help); diff --git a/modules/python/bindings/include/core/display.hpp b/modules/python/bindings/include/core/display.hpp new file mode 100644 index 0000000000..a9f7ceab1a --- /dev/null +++ b/modules/python/bindings/include/core/display.hpp @@ -0,0 +1,104 @@ +/* + * 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: + * Python bindings. + */ + +#ifndef VISP_PYTHON_CORE_DISPLAY_HPP +#define VISP_PYTHON_CORE_DISPLAY_HPP + +#include +#include +#include + +#include +#include + +namespace py = pybind11; + +void bindings_vpDisplay(py::class_> &pyDisplay) +{ + + pyDisplay.def_static("displayCrosses", + [](const vpImage &I, + const py::array_t &is, + const py::array_t &js, + unsigned int size, const vpColor &color, unsigned int thickness = 1) { + + py::buffer_info bufi = is.request(), bufj = js.request(); + if (bufi.ndim != bufj.ndim || bufi.shape != bufj.shape) { + std::stringstream ss; + ss << "is and js must have the same number of dimensions and same number of elements, but got is = " << shape_to_string(bufi.shape); + ss << "and js = " << shape_to_string(bufj.shape); + throw std::runtime_error(ss.str()); + } + + const int *i_ptr = static_cast(bufi.ptr); + const int *j_ptr = static_cast(bufj.ptr); + + for (py::ssize_t i = 0; i < bufi.size; ++i) { + vpDisplay::displayCross(I, i_ptr[i], j_ptr[i], size, color, thickness); + } + + }); + + pyDisplay.def_static("displayLines", + [](const vpImage &I, + const py::array_t &is1, + const py::array_t &js1, + const py::array_t &is2, + const py::array_t &js2, + const vpColor &color, unsigned int thickness = 1, bool segment = true) { + + py::buffer_info bufi1 = is1.request(), bufj1 = js1.request(); + py::buffer_info bufi2 = is2.request(), bufj2 = js2.request(); + + if (bufi1.shape != bufj1.shape || bufi1.shape != bufi2.shape || bufi1.shape != bufj2.shape) { + std::stringstream ss; + ss << "In display lines: numpy arrays must have same dimensions!"; + throw std::runtime_error(ss.str()); + } + + const int *i1_ptr = static_cast(bufi1.ptr); + const int *j1_ptr = static_cast(bufj1.ptr); + + const int *i2_ptr = static_cast(bufi2.ptr); + const int *j2_ptr = static_cast(bufj2.ptr); + + + for (py::ssize_t i = 0; i < bufi1.size; ++i) { + vpDisplay::displayLine(I, i1_ptr[i], j1_ptr[i], i2_ptr[i], j2_ptr[i], color, thickness, segment); + } + + }); + +} + +#endif diff --git a/modules/python/bindings/include/core/images.hpp b/modules/python/bindings/include/core/images.hpp index 3588fe1e6f..c526e070a3 100644 --- a/modules/python/bindings/include/core/images.hpp +++ b/modules/python/bindings/include/core/images.hpp @@ -57,7 +57,7 @@ void define_get_item_2d_image(py::class_, std::shared_ptr> { pyClass.def("__getitem__", [](const vpImage &self, std::pair pair) -> T { int i = pair.first, j = pair.second; - const int rows = (int)self.getHeight(), cols = (int)self.getRows(); + const int rows = (int)self.getRows(), cols = (int)self.getCols(); if (i >= rows || j >= cols || i < -rows || j < -cols) { std::stringstream ss; ss << "Invalid indexing into a 2D image: got indices " << shape_to_string({ i, j }) @@ -94,8 +94,170 @@ void define_get_item_2d_image(py::class_, std::shared_ptr> } /* - * vpImage + * Image 2D indexing */ +template +void define_set_item_2d_image(py::class_, std::shared_ptr>> &pyClass, unsigned int componentsPerPixel) +{ + pyClass.def("__setitem__", [](vpImage &self, std::pair pair, const T &value) { + int i = pair.first, j = pair.second; + const int rows = (int)self.getRows(), cols = (int)self.getCols(); + if (i >= rows || j >= cols || i < -rows || j < -cols) { + std::stringstream ss; + ss << "Invalid indexing into a 2D image: got indices " << shape_to_string({ i, j }) + << " but image has dimensions " << shape_to_string({ rows, cols }); + throw std::runtime_error(ss.str()); + } + if (i < 0) { + i = rows + i; + } + if (j < 0) { + j = cols + j; + } + self[i][j] = value; + }); + pyClass.def("__setitem__", [](vpImage &self, int i, const T &value) { + const int rows = (int)self.getRows(); + if (i >= rows || i < -rows) { + std::stringstream ss; + ss << "Invalid indexing into a 2D image: got indices (" << i << ", :)" + << " but image has dimensions " << shape_to_string({ rows, self.getCols() }); + throw std::runtime_error(ss.str()); + } + if (i < 0) { + i = rows + i; + } + T *row = self[i]; + for (unsigned int j = 0; j < self.getCols(); ++j) { + row[j] = value; + } + }); + pyClass.def("__setitem__", [](vpImage &self, py::slice slice, const T &value) { + int rowStart, rowEnd, rowStep; + std::tie(rowStart, rowEnd, rowStep, std::ignore) = solveSliceIndices(slice, self.getRows()); + for (int i = rowStart; i < rowEnd; i += rowStep) { + T *row = self[i]; + for (unsigned int j = 0; j < self.getCols(); ++j) { + row[j] = value; + } + } + }); + pyClass.def("__setitem__", [](vpImage &self, std::tuple slices, const T &value) { + py::slice sliceRows, sliceCols; + int rowStart, rowEnd, rowStep; + int colStart, colEnd, colStep; + std::tie(sliceRows, sliceCols) = slices; + std::tie(rowStart, rowEnd, rowStep, std::ignore) = solveSliceIndices(sliceRows, self.getRows()); + std::tie(colStart, colEnd, colStep, std::ignore) = solveSliceIndices(sliceCols, self.getCols()); + + for (int i = rowStart; i < rowEnd; i += rowStep) { + T *row = self[i]; + for (int j = colStart; j < colEnd; j += colStep) { + row[j] = value; + } + } + }); + + + if (componentsPerPixel == 1) { + pyClass.def("__setitem__", [](vpImage &self, int row, py::array_t &values) { + if (row < 0) { + row = self.getRows() + row; + } + + if (row > static_cast(self.getRows())) { + throw std::runtime_error("Invalid row index when assigning to image"); + } + + // Copy the array into each row (same values in each row) + py::buffer_info valuesInfo = values.request(); + if (valuesInfo.ndim == 1) { + if (valuesInfo.shape[0] != self.getCols()) { + throw std::runtime_error("Number of image columns and NumPy array dimension do not match"); + } + + const NpRep *value_ptr = static_cast(valuesInfo.ptr); + + T *row_ptr = self[row]; + for (unsigned int j = 0; j < self.getCols(); ++j) { + row_ptr[j] = value_ptr[j]; + } + } + else { + throw std::runtime_error("Cannot write into image row with a multidimensional array"); + } + }); + pyClass.def("__setitem__", [](vpImage &self, py::slice sliceRows, py::array_t &values) { + int rowStart, rowEnd, rowStep, numRows; + std::tie(rowStart, rowEnd, rowStep, numRows) = solveSliceIndices(sliceRows, self.getRows()); + + py::buffer_info valuesInfo = values.request(); + + // Copy the array into each row (same values in each row) + if (valuesInfo.ndim == 1) { + if (valuesInfo.shape[0] != self.getCols()) { + throw std::runtime_error("Number of image columns and NumPy array dimension do not match"); + } + + const NpRep *value_ptr = static_cast(valuesInfo.ptr); + + for (int i = rowStart; i < rowEnd; i += rowStep) { + T *row = self[i]; + unsigned int k = 0; + for (unsigned int j = 0; j < self.getCols(); ++j) { + row[j] = value_ptr[k++]; + } + } + } + // 2D array to 2D array + else if (valuesInfo.ndim == 2) { + if (valuesInfo.shape[0] != numRows || valuesInfo.shape[1] != self.getCols()) { + throw std::runtime_error("Indexing into 2D image: NumPy array has wrong size"); + } + const NpRep *value_ptr = static_cast(valuesInfo.ptr); + + unsigned int k = 0; + for (int i = rowStart; i < rowEnd; i += rowStep) { + T *row = self[i]; + + for (unsigned int j = 0; j < self.getCols(); j++) { + row[j] = value_ptr[k++]; + } + } + } + else { + throw std::runtime_error("Cannot write into 2D raw type image with multidimensional NumPy array that has more than 2 dimensions"); + } + }); + + } + + // Handle vprgba/vprgbf + if (componentsPerPixel > 1) { + // pyClass.def("__setitem__", [](vpImage &self, std::tuple slices, const T &value) { + // py::slice sliceRows, sliceCols; + // int rowStart, rowEnd, rowStep; + // int colStart, colEnd, colStep; + // std::tie(sliceRows, sliceCols) = slices; + // std::tie(rowStart, rowEnd, rowStep) = solveSliceIndices(sliceRows, self.getRows()); + // std::tie(colStart, colEnd, colStep) = solveSliceIndices(sliceCols, self.getCols()); + + // for (int i = rowStart; i < rowEnd; i += rowStep) { + // T *row = self[i]; + // for (unsigned int j = colStart; j < colEnd; j += colStep) { + // row[j] = value; + // } + // } + // }); + + + } +} + + + /* + * vpImage + */ template typename std::enable_if::value, void>::type bindings_vpImage(py::class_, std::shared_ptr>> &pyImage) @@ -121,6 +283,7 @@ Construct an image by **copying** a 2D numpy array. )doc", py::arg("np_array")); define_get_item_2d_image(pyImage); + define_set_item_2d_image(pyImage, 1); pyImage.def("__repr__", [](const vpImage &self) -> std::string { std::stringstream ss; @@ -166,6 +329,8 @@ where the 4 denotes the red, green, blue and alpha components of the image. )doc", py::arg("np_array")); define_get_item_2d_image(pyImage); + define_set_item_2d_image(pyImage, sizeof(T) / sizeof(NpRep)); + pyImage.def("__repr__", [](const vpImage &self) -> std::string { std::stringstream ss; @@ -210,7 +375,9 @@ where the 3 denotes the red, green and blue components of the image. :param np_array: The numpy array to copy. )doc", py::arg("np_array")); + define_get_item_2d_image(pyImage); + define_set_item_2d_image(pyImage, sizeof(T) / sizeof(NpRep)); pyImage.def("__repr__", [](const vpImage &self) -> std::string { std::stringstream ss; diff --git a/modules/python/bindings/include/core/utils.hpp b/modules/python/bindings/include/core/utils.hpp index 11fcea004e..baf24cea97 100644 --- a/modules/python/bindings/include/core/utils.hpp +++ b/modules/python/bindings/include/core/utils.hpp @@ -135,4 +135,58 @@ void copy_data_from_np(np_array_cf src, Item *dest) } +std::tuple solveSliceIndices(py::slice slice, unsigned int size) +{ + + py::handle start = slice.attr("start"), end = slice.attr("stop"), step = slice.attr("step"); + + int startI = 0, endI = size, stepI = 1; + if (!start.is(py::none())) { + startI = py::cast(start); + if (startI < 0) { + startI = size + startI; + } + + if (startI >= static_cast(size)) { + throw std::runtime_error("Invalid slice indexing out of array"); + } + } + + if (!end.is(py::none())) { + endI = py::cast(end); + if (endI < 0) { + endI = size + endI; + } + + if (endI > static_cast(size)) { + throw std::runtime_error("Invalid slice indexing out of array"); + } + } + + if (!step.is(py::none())) { + stepI = py::cast(step); + if (stepI <= 0) { + throw std::runtime_error("Slice indexing: negative or zero step not supported!"); + } + } + + if (endI < startI) { + throw std::runtime_error("Slice indexing: end index is lower than start index"); + } + + int count; + + if (stepI > endI - startI) { + count = 1; + } + else { + int t = (endI - startI) / stepI; + int endS = startI + t * stepI; + count = (endS == endI) ? t : t + 1; + } + + return std::make_tuple(startI, endI, stepI, count); +} + + #endif diff --git a/modules/python/bindings/include/detection.hpp b/modules/python/bindings/include/detection.hpp new file mode 100644 index 0000000000..2678193257 --- /dev/null +++ b/modules/python/bindings/include/detection.hpp @@ -0,0 +1,61 @@ +/* + * 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: + * Python bindings. + */ + + +#ifndef VISP_PYTHON_DETECTION_HPP +#define VISP_PYTHON_DETECTION_HPP + +#include + +#include +#include +#include + + +#include +#include + +namespace py = pybind11; + +void bindings_vpDetectorAprilTag(py::class_, vpDetectorBase> &pyAprilTag) +{ + pyAprilTag.def("detect", + [](vpDetectorAprilTag &self, const vpImage &I, double tagSize, const vpCameraParameters &cam) -> std::tuple> { + std::vector cMos; + bool detected = self.detect(I, tagSize, cam, cMos); + return std::make_tuple(detected, cMos); + }); +} + + +#endif diff --git a/modules/python/bindings/include/rbt.hpp b/modules/python/bindings/include/rbt.hpp new file mode 100644 index 0000000000..293729d423 --- /dev/null +++ b/modules/python/bindings/include/rbt.hpp @@ -0,0 +1,48 @@ +/* + * 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: + * Python bindings. + */ + + +#ifndef VISP_PYTHON_RBT_HPP +#define VISP_PYTHON_RBT_HPP + +#include "rbt/feature_tracker.hpp" +#include "rbt/vo.hpp" +#include "rbt/drift.hpp" +#include "rbt/mask.hpp" + + + + + + +#endif diff --git a/modules/python/bindings/include/rbt/drift.hpp b/modules/python/bindings/include/rbt/drift.hpp new file mode 100644 index 0000000000..e478b3e5ff --- /dev/null +++ b/modules/python/bindings/include/rbt/drift.hpp @@ -0,0 +1,67 @@ + +#ifndef VISP_PYTHON_RBT_DRIFT_HPP +#define VISP_PYTHON_RBT_DRIFT_HPP + +#include +#include + + +class TrampolineRBDriftDetector : public vpRBDriftDetector +{ +public: + using vpRBDriftDetector::vpRBDriftDetector; + + TrampolineRBDriftDetector() : vpRBDriftDetector() { } + + virtual void update(const vpRBFeatureTrackerInput &previousFrame, const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &cprevTo) + { + pybind11::gil_scoped_acquire gil; // Acquire the GIL while in this scope. + // Try to look up the overridden method on the Python side. + pybind11::function override = pybind11::get_override(this, "update"); + if (override) { // method is found + // Pybind seems to copy the frames, so we pass the pointers + override(&previousFrame, &frame, &cTo, &cprevTo); + } + } + + virtual double getScore() const VP_OVERRIDE + { + PYBIND11_OVERRIDE_PURE( + double, /* Return type */ + vpRBDriftDetector, /* Parent class */ + getScore, /* Name of function in C++ (must match Python name) */ + ); + } + + virtual bool hasDiverged() const VP_OVERRIDE + { + PYBIND11_OVERRIDE_PURE( + double, /* Return type */ + vpRBDriftDetector, /* Parent class */ + hasDiverged, /* Name of function in C++ (must match Python name) */ + ); + } + + virtual void display(const vpImage &I) VP_OVERRIDE + { + PYBIND11_OVERRIDE_PURE( + void, /* Return type */ + vpRBDriftDetector, /* Parent class */ + display, /* Name of function in C++ (must match Python name) */ + I + ); + } + + +#if defined(VISP_HAVE_NLOHMANN_JSON) + virtual void loadJsonConfiguration(const nlohmann::json &) VP_OVERRIDE + { + + } +#endif + +}; + + + +#endif diff --git a/modules/python/bindings/include/rbt/feature_tracker.hpp b/modules/python/bindings/include/rbt/feature_tracker.hpp new file mode 100644 index 0000000000..e51d96aa64 --- /dev/null +++ b/modules/python/bindings/include/rbt/feature_tracker.hpp @@ -0,0 +1,157 @@ + +#ifndef VISP_PYTHON_RBT_FEATURE_TRACKER_HPP +#define VISP_PYTHON_RBT_FEATURE_TRACKER_HPP + +#include +#include + + +class TrampolineRBFeatureTracker : public vpRBFeatureTracker +{ +public: + using vpRBFeatureTracker::vpRBFeatureTracker; + + TrampolineRBFeatureTracker() : vpRBFeatureTracker() { } + + + virtual bool requiresRGB() const VP_OVERRIDE + { + PYBIND11_OVERRIDE_PURE( + bool, /* Return type */ + vpRBFeatureTracker, /* Parent class */ + requiresRGB, /* Name of function in C++ (must match Python name) */ + ); + } + virtual bool requiresDepth() const VP_OVERRIDE + { + PYBIND11_OVERRIDE_PURE( + bool, /* Return type */ + vpRBFeatureTracker, /* Parent class */ + requiresDepth, /* Name of function in C++ (must match Python name) */ + ); + } + virtual bool requiresSilhouetteCandidates() const VP_OVERRIDE + { + PYBIND11_OVERRIDE_PURE( + bool, /* Return type */ + vpRBFeatureTracker, /* Parent class */ + requiresSilhouetteCandidates, /* Name of function in C++ (must match Python name) */ + ); + } + virtual void onTrackingIterStart() VP_OVERRIDE + { + PYBIND11_OVERRIDE_PURE( + void, /* Return type */ + vpRBFeatureTracker, /* Parent class */ + onTrackingIterStart, /* Name of function in C++ (must match Python name) */ + ); + } + virtual void onTrackingIterEnd() VP_OVERRIDE + { + PYBIND11_OVERRIDE_PURE( + void, /* Return type */ + vpRBFeatureTracker, /* Parent class */ + onTrackingIterEnd, /* Name of function in C++ (must match Python name) */ + ); + } + virtual void extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) + VP_OVERRIDE + { + pybind11::gil_scoped_acquire gil; // Acquire the GIL while in this scope. + // Try to look up the overridden method on the Python side. + pybind11::function override = pybind11::get_override(this, "extractFeatures"); + if (override) { // method is found + // Pybind seems to copy the frames, so we pass the pointers + override(&frame, &previousFrame, cMo); + } + } + virtual void trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) + VP_OVERRIDE + { + pybind11::gil_scoped_acquire gil; // Acquire the GIL while in this scope. + // Try to look up the overridden method on the Python side. + pybind11::function override = pybind11::get_override(this, "trackFeatures"); + if (override) { // method is found + // Pybind seems to copy the frames, so we pass the pointers + override(&frame, &previousFrame, cMo); + } + } + + virtual void initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE + { + pybind11::gil_scoped_acquire gil; // Acquire the GIL while in this scope. + // Try to look up the overridden method on the Python side. + pybind11::function override = pybind11::get_override(this, "initVVS"); + if (override) { // method is found + // Pybind seems to copy the frames, so we pass the pointers + override(&frame, &previousFrame, cMo); + } + } + virtual void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) VP_OVERRIDE + { + pybind11::gil_scoped_acquire gil; // Acquire the GIL while in this scope. + // Try to look up the overridden method on the Python side. + pybind11::function override = pybind11::get_override(this, "computeVVSIter"); + if (override) { // method is found + // Pybind seems to copy the frames, so we pass the pointers + override(&frame, cMo, iteration); + } + } + virtual void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth) const VP_OVERRIDE + { + pybind11::gil_scoped_acquire gil; // Acquire the GIL while in this scope. + // Try to look up the overridden method on the Python side. + pybind11::function override = pybind11::get_override(this, "display"); + if (override) { // method is found + // Pybind seems to copy the frames, so we pass the pointers + override(cam, &I, &IRGB, &depth); + } + } + virtual const vpMatrix getCovariance() const VP_OVERRIDE + { + PYBIND11_OVERRIDE( + vpMatrix, /* Return type */ + vpRBFeatureTracker, /* Parent class */ + getCovariance, /* Name of function in C++ (must match Python name) */ + + ); + } + virtual void updateCovariance(const double lambda) VP_OVERRIDE + { + PYBIND11_OVERRIDE( + void, /* Return type */ + vpRBFeatureTracker, /* Parent class */ + updateCovariance, /* Name of function in C++ (must match Python name) */ + lambda + ); + } + virtual double getVVSTrackerWeight() const VP_OVERRIDE + { + PYBIND11_OVERRIDE( + double, /* Return type */ + vpRBFeatureTracker, /* Parent class */ + getVVSTrackerWeight, /* Name of function in C++ (must match Python name) */ + ); + } + virtual vpMatrix getLTL() const VP_OVERRIDE + { + PYBIND11_OVERRIDE( + vpMatrix, /* Return type */ + vpRBFeatureTracker, /* Parent class */ + getLTL, + ); + } + virtual vpColVector getLTR() const VP_OVERRIDE + { + PYBIND11_OVERRIDE( + vpColVector, /* Return type */ + vpRBFeatureTracker, /* Parent class */ + getLTR, /* Name of function in C++ (must match Python name) */ + ); + } + +}; + + + +#endif diff --git a/modules/python/bindings/include/rbt/mask.hpp b/modules/python/bindings/include/rbt/mask.hpp new file mode 100644 index 0000000000..944fc78d7d --- /dev/null +++ b/modules/python/bindings/include/rbt/mask.hpp @@ -0,0 +1,49 @@ + +#ifndef VISP_PYTHON_RBT_MASK_HPP +#define VISP_PYTHON_RBT_MASK_HPP + +#include +#include +#include + + +class TrampolineObjectMask : public vpObjectMask +{ +public: + using vpObjectMask::vpObjectMask; + + TrampolineObjectMask() : vpObjectMask() { } + + virtual void updateMask(const vpRBFeatureTrackerInput &frame, + const vpRBFeatureTrackerInput &previousFrame, + vpImage &mask) VP_OVERRIDE + { + pybind11::gil_scoped_acquire gil; // Acquire the GIL while in this scope. + // Try to look up the overridden method on the Python side. + pybind11::function override = pybind11::get_override(this, "updateMask"); + if (override) { // method is found + override(&frame, &previousFrame, &mask); + } + } + + virtual void display(const vpImage &mask, vpImage &Imask) const VP_OVERRIDE + { + PYBIND11_OVERRIDE_PURE( + void, /* Return type */ + vpObjectMask, /* Parent class */ + display, /* Name of function in C++ (must match Python name) */ + mask, Imask + ); + } + +#if defined(VISP_HAVE_NLOHMANN_JSON) + virtual void loadJsonConfiguration(const nlohmann::json &) VP_OVERRIDE + { + + } +#endif +}; + + + +#endif diff --git a/modules/python/bindings/include/rbt/vo.hpp b/modules/python/bindings/include/rbt/vo.hpp new file mode 100644 index 0000000000..cbe915be6f --- /dev/null +++ b/modules/python/bindings/include/rbt/vo.hpp @@ -0,0 +1,49 @@ + +#ifndef VISP_PYTHON_RBT_VO_HPP +#define VISP_PYTHON_RBT_VO_HPP + +#include +#include + + +class TrampolineRBVisualOdometry : public vpRBVisualOdometry +{ +public: + using vpRBVisualOdometry::vpRBVisualOdometry; + + TrampolineRBVisualOdometry() : vpRBVisualOdometry() { } + + virtual void compute(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame) VP_OVERRIDE + { + pybind11::gil_scoped_acquire gil; // Acquire the GIL while in this scope. + // Try to look up the overridden method on the Python side. + pybind11::function override = pybind11::get_override(this, "compute"); + if (override) { // method is found + // Pybind seems to copy the frames, so we pass the pointers + override(&frame, &previousFrame); + } + } + + virtual vpHomogeneousMatrix getCameraMotion() const VP_OVERRIDE + { + PYBIND11_OVERRIDE_PURE( + vpHomogeneousMatrix, /* Return type */ + vpRBVisualOdometry, /* Parent class */ + getCameraMotion, /* Name of function in C++ (must match Python name) */ + + ); + } + virtual vpHomogeneousMatrix getCameraPose() const VP_OVERRIDE + { + PYBIND11_OVERRIDE_PURE( + vpHomogeneousMatrix, /* Return type */ + vpRBVisualOdometry, /* Parent class */ + getCameraPose, /* Name of function in C++ (must match Python name) */ + + ); + } +}; + + + +#endif diff --git a/modules/python/bindings/include/vision.hpp b/modules/python/bindings/include/vision.hpp new file mode 100644 index 0000000000..a18078554a --- /dev/null +++ b/modules/python/bindings/include/vision.hpp @@ -0,0 +1,68 @@ +/* + * 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: + * Python bindings. + */ + + +#ifndef VISP_PYTHON_VISION_HPP +#define VISP_PYTHON_VISION_HPP + + +#include +#include +#include +#include +#include + +namespace py = pybind11; + +void bindings_vpPose(py::class_> &pyPose) +{ + pyPose.def("computePose", + [](vpPose &self, vpPose::vpPoseMethodType method, vpHomogeneousMatrix &cMo) -> bool { + return self.computePose(method, cMo); + }); + + pyPose.def_static("computePlanarObjectPoseFromRGBD", + [](const vpImage &depthMap, const std::vector &corners, + const vpCameraParameters &colorIntrinsics, + const std::vector &point3d) -> std::tuple { + double confidence = 0.0; + vpHomogeneousMatrix cMo; + bool valid = vpPose::computePlanarObjectPoseFromRGBD(depthMap, corners, colorIntrinsics, point3d, cMo, &confidence); + return std::make_tuple(valid, cMo, confidence); + }); + + +} + + +#endif diff --git a/modules/python/config/core.json b/modules/python/config/core.json index f52116f0dd..3ca4110ba2 100644 --- a/modules/python/config/core.json +++ b/modules/python/config/core.json @@ -1,12 +1,29 @@ { - "ignored_headers": ["vpGEMM.h", "vpDebug.h", "vpEndian.h"], - "ignored_classes": ["vpException", "vpImageException", "vpTrackingException", - "vpFrameGrabberException", "vpIoException", - "vpDisplayException", "vpMatrixException"], - "user_defined_headers": ["core.hpp"], - "config_includes": ["core_image.json", "core_math.json"], + "ignored_headers": [ + "vpGEMM.h", + "vpDebug.h", + "vpEndian.h" + ], + "ignored_classes": [ + "vpException", + "vpImageException", + "vpTrackingException", + "vpFrameGrabberException", + "vpIoException", + "vpDisplayException", + "vpMatrixException" + ], + "user_defined_headers": [ + "core.hpp" + ], + "config_includes": [ + "core_image.json", + "core_math.json" + ], "header_additional_dependencies": { - "vpUKSigmaDrawerMerwe.h": ["vpUnscentedKalman.h"] + "vpUKSigmaDrawerMerwe.h": [ + "vpUnscentedKalman.h" + ] }, "enums": { "vpMunkres::STEP_T": { @@ -95,7 +112,9 @@ ], "classes": { "vpIoTools": { - "ignored_attributes": ["separator"], + "ignored_attributes": [ + "separator" + ], "methods": [ { "signature": "void readBinaryValueLE(std::ifstream&, int16_t&)", @@ -131,70 +150,118 @@ "static": true, "signature": "void getUserName(std::string&)", "use_default_param_policy": false, - "param_is_input": [false], - "param_is_output": [true] + "param_is_input": [ + false + ], + "param_is_output": [ + true + ] }, { "static": true, "signature": "void getVersion(const std::string&, unsigned int&, unsigned int&, unsigned int&)", "use_default_param_policy": false, - "param_is_input": [true, false, false, false], - "param_is_output": [false, true, true, true] + "param_is_input": [ + true, + false, + false, + false + ], + "param_is_output": [ + false, + true, + true, + true + ] }, { "static": true, "signature": "bool readConfigVar(const std::string&, float&)", "custom_name": "readConfigVarFloat", "use_default_param_policy": false, - "param_is_input": [true, false], - "param_is_output": [false, true] + "param_is_input": [ + true, + false + ], + "param_is_output": [ + false, + true + ] }, { "static": true, "signature": "bool readConfigVar(const std::string&, double&)", "custom_name": "readConfigVarDouble", "use_default_param_policy": false, - "param_is_input": [true, false], - "param_is_output": [false, true] + "param_is_input": [ + true, + false + ], + "param_is_output": [ + false, + true + ] }, { "static": true, "signature": "bool readConfigVar(const std::string&, unsigned int&)", "custom_name": "readConfigVarUnsigned", "use_default_param_policy": false, - "param_is_input": [true, false], - "param_is_output": [false, true] + "param_is_input": [ + true, + false + ], + "param_is_output": [ + false, + true + ] }, { "static": true, "signature": "bool readConfigVar(const std::string&, int&)", "custom_name": "readConfigVarInt", "use_default_param_policy": false, - "param_is_input": [true, false], - "param_is_output": [false, true] + "param_is_input": [ + true, + false + ], + "param_is_output": [ + false, + true + ] }, { "static": true, "signature": "bool readConfigVar(const std::string&, bool&)", "custom_name": "readConfigVarBoolean", "use_default_param_policy": false, - "param_is_input": [true, false], - "param_is_output": [false, true] + "param_is_input": [ + true, + false + ], + "param_is_output": [ + false, + true + ] }, { "static": true, "signature": "bool readConfigVar(const std::string&, std::string&)", "custom_name": "readConfigVarString", "use_default_param_policy": false, - "param_is_input": [true, false], - "param_is_output": [false, true] + "param_is_input": [ + true, + false + ], + "param_is_output": [ + false, + true + ] } ] }, - "vpPolygon": { - "methods": - [ + "methods": [ { "static": true, "signature": "bool isInside(const std::vector&, const double&, const double&, const vpPolygon::PointInPolygonMethod&)", @@ -208,61 +275,121 @@ "static": true, "signature": "void getClippedPolygon(const std::vector&, std::vector&, const vpHomogeneousMatrix&, const unsigned int&, const vpCameraParameters&, const double&, const double&)", "use_default_param_policy": false, - "param_is_input": [true, false, true, true, true, true, true], - "param_is_output": [false, true, false, false, false, false, false] + "param_is_input": [ + true, + false, + true, + true, + true, + true, + true + ], + "param_is_output": [ + false, + true, + false, + false, + false, + false, + false + ] }, { "static": false, "signature": "void getPolygonClipped(std::vector&)", "use_default_param_policy": false, - "param_is_input": [false], - "param_is_output": [true] + "param_is_input": [ + false + ], + "param_is_output": [ + true + ] }, { "static": false, "signature": "void getPolygonClipped(std::vector>&)", "custom_name": "getPolygonClippedWithInfo", "use_default_param_policy": false, - "param_is_input": [false], - "param_is_output": [true] + "param_is_input": [ + false + ], + "param_is_output": [ + true + ] }, { "static": false, "signature": "void getRoiClipped(const vpCameraParameters&, std::vector>&, const vpHomogeneousMatrix&)", "use_default_param_policy": false, - "param_is_input": [true, false, true], - "param_is_output": [false, true, false] + "param_is_input": [ + true, + false, + true + ], + "param_is_output": [ + false, + true, + false + ] }, { "static": false, "signature": "void getRoiClipped(const vpCameraParameters&, std::vector>&)", "use_default_param_policy": false, - "param_is_input": [true, false], - "param_is_output": [false, true] + "param_is_input": [ + true, + false + ], + "param_is_output": [ + false, + true + ] }, { "static": false, "signature": "void getRoiClipped(const vpCameraParameters&, std::vector&, const vpHomogeneousMatrix&)", "use_default_param_policy": false, - "param_is_input": [true, false, true], - "param_is_output": [false, true, false] + "param_is_input": [ + true, + false, + true + ], + "param_is_output": [ + false, + true, + false + ] }, { "static": false, "signature": "void getRoiClipped(const vpCameraParameters&, std::vector&)", "use_default_param_policy": false, - "param_is_input": [true, false], - "param_is_output": [false, true] + "param_is_input": [ + true, + false + ], + "param_is_output": [ + false, + true + ] }, { "static": true, "signature": "void getMinMaxRoi(const std::vector&, int&, int&, int&, int&)", "use_default_param_policy": false, "param_is_input": [ - true, false, false, false, false + true, + false, + false, + false, + false ], "param_is_output": [ - false, true, true, true, true + false, + true, + true, + true, + true ] } ] @@ -277,8 +404,7 @@ ] }, "vpPoint": { - "methods": - [ + "methods": [ { "static": false, "ignore": true, @@ -289,7 +415,6 @@ "ignore": true, "signature": "void getWorldCoordinates(double&, double&, double&)" } - ] }, "vpRect": { @@ -298,14 +423,19 @@ "static": false, "signature": "void getCenter(double&, double&)", "use_default_param_policy": false, - "param_is_input": [false,false], - "param_is_output": [true, true] + "param_is_input": [ + false, + false + ], + "param_is_output": [ + true, + true + ] } ] }, "vpBSpline": { - "methods": - [ + "methods": [ { "static": true, "signature": "unsigned int findSpan(double, unsigned int, const std::vector &)", @@ -331,21 +461,28 @@ "static": false, "signature": "void get_knots(std::list&)", "use_default_param_policy": false, - "param_is_input": [false], - "param_is_output": [true] + "param_is_input": [ + false + ], + "param_is_output": [ + true + ] }, { "static": false, "signature": "void get_controlPoints(std::list&)", "use_default_param_policy": false, - "param_is_input": [false], - "param_is_output": [true] + "param_is_input": [ + false + ], + "param_is_output": [ + true + ] } ] }, "vpQuadProg": { - "methods": - [ + "methods": [ { "static": true, "signature": "bool solveQPe(const vpMatrix &, const vpColVector &, vpMatrix, vpColVector, vpColVector &, const double &)", @@ -358,17 +495,17 @@ { "static": true, "signature": "void convertToOpenCV(const std::vector&, std::vector&, bool)", - "ignore" :true + "ignore": true }, { "static": true, "signature": "void convertToOpenCV(const std::vector&, std::vector&, bool)", - "ignore" :true + "ignore": true }, { "static": true, "signature": "void convertToOpenCV(const std::vector&, std::vector&)", - "ignore" :true + "ignore": true }, { "static": true, @@ -413,8 +550,8 @@ ] }, "vpDisplay": { - "methods": - [ + "additional_bindings": "bindings_vpDisplay", + "methods": [ { "static": true, "signature": "unsigned int getDownScalingFactor(const vpImage &)", @@ -527,29 +664,85 @@ "static": true, "signature": "void convertEllipse(const vpCameraParameters&, const vpImagePoint&, double, double, double, double&, double&, double&, double&, double&)", "use_default_param_policy": false, - "param_is_input": [true, true, true, true, true, false, false, false, false, false], - "param_is_output": [false, false, false, false, false, true, true, true, true, true] + "param_is_input": [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false + ], + "param_is_output": [ + false, + false, + false, + false, + false, + true, + true, + true, + true, + true + ] }, { "static": true, "signature": "void convertLine(const vpCameraParameters&, const double&, const double&, double&, double&)", "use_default_param_policy": false, - "param_is_input": [true,true,true,false,false], - "param_is_output": [false,false,false,true,true] + "param_is_input": [ + true, + true, + true, + false, + false + ], + "param_is_output": [ + false, + false, + false, + true, + true + ] }, { "static": true, "signature": "void convertPoint(const vpCameraParameters&, const double&, const double&, double&, double&)", "use_default_param_policy": false, - "param_is_input": [true,true,true,false,false], - "param_is_output": [false,false,false,true,true] + "param_is_input": [ + true, + true, + true, + false, + false + ], + "param_is_output": [ + false, + false, + false, + true, + true + ] }, { "static": true, "signature": "void convertPoint(const vpCameraParameters&, const vpImagePoint&, double&, double&)", "use_default_param_policy": false, - "param_is_input": [true,true,false,false], - "param_is_output": [false,false,true,true] + "param_is_input": [ + true, + true, + false, + false + ], + "param_is_output": [ + false, + false, + true, + true + ] }, { "static": true, @@ -572,7 +765,6 @@ "ignore": true } ] - }, "vpMeterPixelConversion": { "additional_bindings": "bindings_vpMeterPixelConversion", @@ -581,50 +773,126 @@ "static": true, "signature": "void convertEllipse(const vpCameraParameters&, const vpImagePoint&, double, double, double, double&, double&, double&, double&, double&)", "use_default_param_policy": false, - "param_is_input": [true, true, true, true, true, false, false, false, false, false], - "param_is_output": [false, false, false, false, false, true, true, true, true, true] + "param_is_input": [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false + ], + "param_is_output": [ + false, + false, + false, + false, + false, + true, + true, + true, + true, + true + ] }, { "static": true, "signature": "void convertEllipse(const vpCameraParameters&, const vpSphere&, vpImagePoint&, double&, double&, double&)", "use_default_param_policy": false, "param_is_input": [ - true, true, true, false, false, false + true, + true, + true, + false, + false, + false ], "param_is_output": [ - false, false, false, true, true, true + false, + false, + false, + true, + true, + true ] }, { "static": true, "signature": "void convertLine(const vpCameraParameters&, const double&, const double&, double&, double&)", "use_default_param_policy": false, - "param_is_input": [true,true,true,false,false], - "param_is_output": [false,false,false,true,true] + "param_is_input": [ + true, + true, + true, + false, + false + ], + "param_is_output": [ + false, + false, + false, + true, + true + ] }, { "static": true, "signature": "void convertPoint(const vpCameraParameters&, const double&, const double&, double&, double&)", "use_default_param_policy": false, - "param_is_input": [true,true,true,false,false], - "param_is_output": [false,false,false,true,true] + "param_is_input": [ + true, + true, + true, + false, + false + ], + "param_is_output": [ + false, + false, + false, + true, + true + ] }, { "static": true, "signature": "void convertPoint(const vpCameraParameters&, const vpImagePoint&, double&, double&)", "use_default_param_policy": false, - "param_is_input": [true,true,false,false], - "param_is_output": [false,false,true,true] + "param_is_input": [ + true, + true, + false, + false + ], + "param_is_output": [ + false, + false, + true, + true + ] }, { "static": true, "signature": "void convertEllipse(const vpCameraParameters&, const vpCircle&, vpImagePoint&, double&, double&, double&)", "use_default_param_policy": false, "param_is_input": [ - true, true, true, true, true, true + true, + true, + true, + true, + true, + true ], "param_is_output": [ - false, false, false, true, true, true + false, + false, + false, + true, + true, + true ] }, { @@ -632,10 +900,28 @@ "signature": "void convertEllipse(const vpCameraParameters&, double, double, double, double, double, vpImagePoint&, double&, double&, double&)", "use_default_param_policy": false, "param_is_input": [ - true, true, true, true, true, true, true, false, false, false + true, + true, + true, + true, + true, + true, + true, + false, + false, + false ], "param_is_output": [ - false, false, false, false, false, false, false, true, true, true + false, + false, + false, + false, + false, + false, + false, + true, + true, + true ] }, { @@ -681,8 +967,22 @@ "static": true, "signature": "void computeIntersectionPoint(const vpCircle&, const vpCameraParameters&, const double&, const double&, double&, double&)", "use_default_param_policy": false, - "param_is_input": [true, true, true, true, false, false], - "param_is_output": [false, false, false, false, true, true] + "param_is_input": [ + true, + true, + true, + true, + false, + false + ], + "param_is_output": [ + false, + false, + false, + false, + true, + true + ] } ] }, @@ -692,14 +992,18 @@ "static": true, "signature": "double derivativeFilterX(const vpImage&, unsigned int, unsigned int)", "specializations": [ - ["TypeFilterable"] + [ + "TypeFilterable" + ] ] }, { "static": true, "signature": "double derivativeFilterY(const vpImage&, unsigned int, unsigned int)", "specializations": [ - ["TypeFilterable"] + [ + "TypeFilterable" + ] ] } ] @@ -709,32 +1013,56 @@ { "static": true, "signature": "void dilatation(vpImage&, const int&)", - "specializations": [["TypeErodableDilatable"]] + "specializations": [ + [ + "TypeErodableDilatable" + ] + ] }, { "static": true, "signature": "void erosion(vpImage&, Type, Type, vpImageMorphology::vpConnexityType)", - "specializations": [["TypeErodableDilatable"]] + "specializations": [ + [ + "TypeErodableDilatable" + ] + ] }, { "static": true, "signature": "void erosion(vpImage&, const vpImageMorphology::vpConnexityType&)", - "specializations": [["TypeErodableDilatable"]] + "specializations": [ + [ + "TypeErodableDilatable" + ] + ] }, { "static": true, "signature": "void dilatation(vpImage&, Type, Type, vpImageMorphology::vpConnexityType)", - "specializations": [["TypeErodableDilatable"]] + "specializations": [ + [ + "TypeErodableDilatable" + ] + ] }, { "static": true, "signature": "void dilatation(vpImage&, const vpImageMorphology::vpConnexityType&)", - "specializations": [["TypeErodableDilatable"]] + "specializations": [ + [ + "TypeErodableDilatable" + ] + ] }, { "static": true, "signature": "void erosion(vpImage&, const int&)", - "specializations": [["TypeErodableDilatable"]] + "specializations": [ + [ + "TypeErodableDilatable" + ] + ] } ] }, @@ -800,15 +1128,29 @@ "static": false, "signature": "int receive(std::string&, std::string&, int)", "use_default_param_policy": false, - "param_is_input": [false, false, true], - "param_is_output": [true, true, false] + "param_is_input": [ + false, + false, + true + ], + "param_is_output": [ + true, + true, + false + ] }, { "static": false, "signature": "int receive(std::string&, int)", "use_default_param_policy": false, - "param_is_input": [false, true], - "param_is_output": [true, false] + "param_is_input": [ + false, + true + ], + "param_is_output": [ + true, + false + ] } ] }, @@ -817,7 +1159,11 @@ { "static": true, "signature": "std::vector shuffleVector(const std::vector&)", - "specializations": [["TypePythonScalar"]] + "specializations": [ + [ + "TypePythonScalar" + ] + ] } ] }, @@ -827,15 +1173,23 @@ "static": false, "signature": "unsigned getPeaks(std::list&)", "use_default_param_policy": false, - "param_is_input": [false], - "param_is_output": [true] + "param_is_input": [ + false + ], + "param_is_output": [ + true + ] }, { "static": false, "signature": "unsigned getValey(std::list&)", "use_default_param_policy": false, - "param_is_input": [false], - "param_is_output": [true] + "param_is_input": [ + false + ], + "param_is_output": [ + true + ] }, { "static": false, diff --git a/modules/python/config/detection.json b/modules/python/config/detection.json index 78946149cf..10e869f257 100644 --- a/modules/python/config/detection.json +++ b/modules/python/config/detection.json @@ -1,3 +1,13 @@ { - "required_headers": ["visp3/core/vpPoint.h"] -} \ No newline at end of file + "required_headers": [ + "visp3/core/vpPoint.h" + ], + "user_defined_headers": [ + "detection.hpp" + ], + "classes": { + "vpDetectorAprilTag": { + "additional_bindings": "bindings_vpDetectorAprilTag" + } + } +} diff --git a/modules/python/config/rbt.json b/modules/python/config/rbt.json new file mode 100644 index 0000000000..4fa9495e8e --- /dev/null +++ b/modules/python/config/rbt.json @@ -0,0 +1,294 @@ +{ + "ignored_headers": [], + "ignored_classes": [], + "user_defined_headers": [ + "rbt.hpp" + ], + "classes": { + "vpDynamicFactory": { + "specializations": [ + { + "python_name": "DynamicFactoryTracker", + "arguments": [ + "vpRBFeatureTracker" + ] + }, + { + "python_name": "DynamicFactoryMask", + "arguments": [ + "vpObjectMask" + ] + }, + { + "python_name": "DynamicFactoryDrift", + "arguments": [ + "vpRBDriftDetector" + ] + } + ], + "methods": [ + { + "static": false, + "signature": "void registerTypeRaw(const std::string&, const std::function(const std::string&)>&)" + } + ], + "acknowledge_pointer_or_ref_fields": [ + "std::map(const nlohmann::json&)>>", + "std::function" + ] + }, + "vpRBTracker": { + "methods": [ + { + "static": false, + "signature": "const vpRBFeatureTrackerInput& getMostRecentFrame()", + "return_policy": "reference", + "keep_alive": [ + [ + 1, + 0 + ] + ], + "returns_ref_ok": true + }, + { + "static": false, + "signature": "const vpRBTrackerLogger& getLogger()", + "return_policy": "reference", + "keep_alive": [ + [ + 1, + 0 + ] + ], + "returns_ref_ok": true + }, + { + "static": false, + "signature": "vpObjectCentricRenderer& getRenderer()", + "return_policy": "reference", + "keep_alive": [ + [ + 1, + 0 + ] + ], + "returns_ref_ok": true + } + ] + }, + "vpRBRenderData": { + "methods": [ + { + "static": false, + "signature": " vpRBRenderData(vpRBRenderData&&)", + "ignore": true + }, + { + "static": false, + "signature": "vpRBRenderData& operator=(vpRBRenderData&&)", + "ignore": true + } + ] + }, + "vpRBFeatureTrackerInput": { + "methods": [ + { + "static": false, + "signature": " vpRBFeatureTrackerInput(vpRBFeatureTrackerInput&&)", + "ignore": true + }, + { + "static": false, + "signature": "vpRBFeatureTrackerInput& operator=(vpRBFeatureTrackerInput&&)", + "ignore": true + } + ] + }, + "vpRBFeatureTracker": { + "trampoline": "TrampolineRBFeatureTracker", + "use_publicist": true, + "methods": [ + { + "static": false, + "signature": "const vpColVector& getWeightedError()", + "return_policy": "reference", + "keep_alive": [ + [ + 1, + 0 + ] + ], + "returns_ref_ok": true + } + ] + }, + "vpRBSilhouetteControlPoint": { + "acknowledge_pointer_or_ref_fields": [ + "const vpMe*", + "const vpCameraParameters*" + ], + "methods": [ + { + "static": false, + "signature": " vpRBSilhouetteControlPoint(const vpRBSilhouetteControlPoint&&)", + "ignore": true + }, + { + "static": false, + "signature": "vpRBSilhouetteControlPoint& operator=(const vpRBSilhouetteControlPoint&&)", + "ignore": true + }, + { + "static": false, + "signature": "const vpLine& getLine()", + "return_policy": "reference", + "keep_alive": [ + [ + 1, + 0 + ] + ], + "returns_ref_ok": true + }, + { + "static": false, + "signature": "const vpFeatureLine& getFeatureLine()", + "return_policy": "reference", + "keep_alive": [ + [ + 1, + 0 + ] + ], + "returns_ref_ok": true + }, + { + "static": false, + "signature": "vpMeSite& getSite()", + "return_policy": "reference", + "keep_alive": [ + [ + 1, + 0 + ] + ], + "returns_ref_ok": true + }, + { + "static": false, + "signature": "const vpMeSite& getSite()", + "return_policy": "reference", + "keep_alive": [ + [ + 1, + 0 + ] + ], + "returns_ref_ok": true + }, + { + "static": false, + "signature": "const vpCameraParameters& getCameraParameters()", + "return_policy": "reference", + "keep_alive": [ + [ + 1, + 0 + ] + ], + "returns_ref_ok": true + } + ] + }, + "vpRBKltTracker": { + "methods": [ + { + "static": false, + "signature": "vpKltOpencv& getKltTracker()", + "return_policy": "reference", + "keep_alive": [ + [ + 1, + 0 + ] + ], + "returns_ref_ok": true + }, + { + "static": false, + "signature": "const vpKltOpencv& getKltTracker()", + "ignore": true + } + ] + }, + "vpRBDriftDetectorFactory": { + "methods": [ + { + "static": true, + "signature": "vpRBDriftDetectorFactory& getFactory()", + "return_policy": "reference", + "returns_ref_ok": true + } + ] + }, + "vpObjectMaskFactory": { + "methods": [ + { + "static": true, + "signature": "vpObjectMaskFactory& getFactory()", + "return_policy": "reference", + "returns_ref_ok": true + } + ] + }, + "vpRBFeatureTrackerFactory": { + "methods": [ + { + "static": true, + "signature": "vpRBFeatureTrackerFactory& getFactory()", + "return_policy": "reference", + "returns_ref_ok": true + } + ] + }, + "vpRBVisualOdometry": { + "trampoline": "TrampolineRBVisualOdometry" + }, + "vpRBDriftDetector": { + "trampoline": "TrampolineRBDriftDetector" + }, + "vpObjectMask": { + "trampoline": "TrampolineObjectMask" + }, + "vpObjectCentricRenderer": { + "methods": [ + { + "static": false, + "signature": "void computeClipping(float&, float&)", + "use_default_param_policy": false, + "param_is_input": [ + false, + false + ], + "param_is_output": [ + true, + true + ] + } + ] + }, + "vpPanda3DDepthGaussianBlur": { + "acknowledge_pointer_or_ref_fields": [ + "const char*" + ] + }, + "vpPanda3DDepthCannyFilter": { + "acknowledge_pointer_or_ref_fields": [ + "const char*" + ] + } + }, + "enums": {}, + "config_includes": [] +} diff --git a/modules/python/config/vision.json b/modules/python/config/vision.json index 80aaea98bc..a620749033 100644 --- a/modules/python/config/vision.json +++ b/modules/python/config/vision.json @@ -1,9 +1,15 @@ { - "ignored_headers": ["vpPoseException.h", "vpCalibrationException.h", "vpPoseFeatures.h"], + "ignored_headers": [ + "vpPoseException.h", + "vpCalibrationException.h", + "vpPoseFeatures.h" + ], + "user_defined_headers": [ + "vision.hpp" + ], "classes": { - "vpHomography" : { - "methods": - [ + "vpHomography": { + "methods": [ { "static": true, "signature": "void computeDisplacement(const vpHomography &, const vpColVector &, vpRotationMatrix &, vpTranslationVector &, vpColVector &)", @@ -20,6 +26,17 @@ "custom_name": "computeHomographyDisplacement" } ] + }, + "vpPose": { + "additional_bindings": "bindings_vpPose", + "methods": [ + { + "static": true, + "signature": "bool computePlanarObjectPoseFromRGBD(const vpImage&, const std::vector&, const vpCameraParameters&, const std::vector&, vpHomogeneousMatrix&, double*)", + "ignore": true, + "custom_implem": true + } + ] } } -} \ No newline at end of file +} diff --git a/modules/python/examples/realsense-mbt.py b/modules/python/examples/realsense-mbt.py index d222f62ff8..9f0a582fb7 100644 --- a/modules/python/examples/realsense-mbt.py +++ b/modules/python/examples/realsense-mbt.py @@ -195,7 +195,7 @@ def cam_from_rs_profile(profile) -> Tuple[CameraParameters, int, int]: tracker.initClick(I, str(mbt_model.init_file)) start_time = time.time() for frame_data in data_generator: - if frame_data.I_depth is not None: + if I_depth is not None: ImageConvert.createDepthHistogram(frame_data.I_depth, I_depth) Display.display(I) diff --git a/modules/python/examples/realsense-rbt.py b/modules/python/examples/realsense-rbt.py new file mode 100644 index 0000000000..be5a301ee4 --- /dev/null +++ b/modules/python/examples/realsense-rbt.py @@ -0,0 +1,227 @@ +############################################################################# +# +# ViSP, open source Visual Servoing Platform software. +# Copyright (C) 2005 - 2023 by Inria. All rights reserved. +# +# This software is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# See the file LICENSE.txt at the root directory of this source +# distribution for additional information about the GNU GPL. +# +# For using ViSP with software that can not be combined with the GNU +# GPL, please contact Inria about acquiring a ViSP Professional +# Edition License. +# +# See https://visp.inria.fr for more information. +# +# This software was developed at: +# Inria Rennes - Bretagne Atlantique +# Campus Universitaire de Beaulieu +# 35042 Rennes Cedex +# France +# +# If you have questions regarding the use of this file, please contact +# Inria at visp@inria.fr +# +# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE +# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. +# +# Description: +# ViSP Python bindings example +# +############################################################################# + +import argparse +from dataclasses import dataclass +from pathlib import Path +from typing import List, Optional, Tuple +import numpy as np +import time +import faulthandler +faulthandler.enable() + + +from visp.core import CameraParameters, HomogeneousMatrix +from visp.core import Color, Display, ImageConvert +from visp.core import ImageGray, ImageUInt16, ImageRGBa, ImageFloat +from visp.io import ImageIo +from visp.rbt import RBTracker, RBFeatureDisplayType, RBFeatureTracker, RBFeatureTrackerInput +from visp.display_utils import get_display +import pyrealsense2 as rs + + +import matplotlib.pyplot as plt + +class PyBaseFeatureTracker(RBFeatureTracker): + def __init__(self): + RBFeatureTracker.__init__(self) + + def requiresRGB(self) -> bool: + return False + def requiresDepth(self) -> bool: + return False + def requiresSilhouetteCandidates(self) -> bool: + return False + + def onTrackingIterStart(self): + self.cov.resize(6, 6) + self.LTL.resize(6, 6) + self.LTR.resize(6) + self.numFeatures = 0 + + def extractFeatures(self, frame: RBFeatureTrackerInput, previousFrame: RBFeatureTrackerInput, cMo: HomogeneousMatrix): + print(frame) + pass + + def trackFeatures(self, frame: RBFeatureTrackerInput, previousFrame: RBFeatureTrackerInput, cMo: HomogeneousMatrix): + pass + + def initVVS(self, frame: RBFeatureTrackerInput, previousFrame: RBFeatureTrackerInput, cMo: HomogeneousMatrix): + print('INITVVS Was called') + pass + + def computeVVSIter(self, frame: RBFeatureTrackerInput, cMo: HomogeneousMatrix, iteration: int): + pass + + def onTrackingIterEnd(self): + pass + + def display(self, cam: CameraParameters, I: ImageGray, IRGB: ImageRGBa, I_depth: ImageGray, type: RBFeatureDisplayType): + pass + +@dataclass +class FrameData: + I: ImageGray + IRGB: ImageRGBa + I_depth: Optional[ImageFloat] + + +def read_data(depth_scale: Optional[float], IRGB: ImageRGBa, I: ImageGray, pipe: rs.pipeline): + use_depth = depth_scale is not None + iteration = 1 + align_to = rs.align(rs.stream.color) + while True: + frames = pipe.wait_for_frames() + frames = align_to.process(frames) + I_np = np.asanyarray(frames.get_color_frame().as_frame().get_data()) + I_np = np.concatenate((I_np, np.ones_like(I_np[..., 0:1], dtype=np.uint8)), axis=-1) + IRGB.resize(I_np.shape[0], I_np.shape[1]) + I_rgba_ref = IRGB.numpy() + I_rgba_ref[...] = I_np + ImageConvert.convert(IRGB, I, 0) + I_depth_float = None + if use_depth: + I_depth_raw = np.asanyarray(frames.get_depth_frame().as_frame().get_data()) + I_depth_float = I_depth_raw.astype(np.float32) * depth_scale + iteration += 1 + yield FrameData(I, IRGB, ImageFloat(I_depth_float)) + + +def cam_from_rs_profile(profile) -> Tuple[CameraParameters, int, int]: + intr = profile.as_video_stream_profile().get_intrinsics() # Downcast to video_stream_profile and fetch intrinsics + return CameraParameters(intr.fx, intr.fy, intr.ppx, intr.ppy), intr.height, intr.width + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('--tracker', type=str, required=True, + help='Path to the json file containing the tracker configuration.') + parser.add_argument('--model', type=str, required=False, + help='Path to the .obj/.bam file describing the CAD model.') + + args = parser.parse_args() + tracker_path: str = args.tracker + assert Path(tracker_path).exists(), 'Tracker file not found' + model_path = args.model + if model_path is not None: + assert Path(model_path).exists(), '3D CAD model file not found' + + # Initialize realsense2 + pipe = rs.pipeline() + config = rs.config() + config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 60) + config.enable_stream(rs.stream.color, 848, 480, rs.format.rgb8, 60) + + cfg = pipe.start(config) + depth_scale = cfg.get_device().first_depth_sensor().get_depth_scale() + + + tracker = RBTracker() + + tracker.loadConfigurationFile(tracker_path) + if model_path is not None: + tracker.setModelPath(model_path) + + custom_feature = PyBaseFeatureTracker() + tracker.addTracker(custom_feature) + + cam_color, color_height, color_width = cam_from_rs_profile(cfg.get_stream(rs.stream.color)) + + tracker.setCameraParameters(cam_color, color_height, color_width) + + # Camera intrinsics + + print('Color intrinsics:', cam_color) + I = ImageGray() + IRGB = ImageRGBa() + I_depth_display = ImageGray() + data_generator = read_data(depth_scale, IRGB, I, pipe) + frame_data = next(data_generator) # Get first frame for init + + # Initialize displays + dI = get_display() + dI.init(I, 0, 0, 'Color image') + + dRGB = get_display() + dRGB.init(IRGB, I.getWidth(), 0, 'Color image') + + I_depth = ImageGray() + dDepth = get_display() + + ImageConvert.createDepthHistogram(frame_data.I_depth, I_depth) + dDepth.init(I_depth, I.getWidth() * 2, 0, 'Depth') + + for frame in data_generator: + Display.display(I) + Display.displayText(I, 50, 0, 'Click to initialize tracking', Color.red) + Display.flush(I) + Display.display(IRGB) + Display.flush(IRGB) + event = Display.getClick(I, blocking=False) + if event: + break + tracker.startTracking() + tracker.initClick(I, tracker_path.replace('.json', '.init'), True) + start_time = time.time() + for frame_data in data_generator: + if frame_data.I_depth is not None: + I_depth_np = I_depth.numpy() + I_depth_np[...] = ((np.minimum(frame_data.I_depth, 0.5) / 0.5) * 255.0).astype(np.uint8) + + displayed = [I, IRGB, I_depth] + + for display_image in displayed: + Display.display(display_image) + Display.displayText(I, 50, 0, 'Click to stop tracking', Color.red) + + # if args.disable_depth: + # tracker.track(I=I, IRGB=IRGB) + # else: + tracker.track(I=frame.I, IRGB=frame_data.IRGB, depth=frame_data.I_depth) + cMo = HomogeneousMatrix() + tracker.getPose(cMo) + + tracker.display(I, IRGB, I_depth, RBFeatureDisplayType.SIMPLE) + Display.displayFrame(I, cMo, cam_color, 0.05, Color.none, 2) + + for display_image in displayed: + Display.flush(display_image) + + + + event = Display.getClick(I, blocking=False) + if event: + break + end_time = time.time() + print(f'total time = {end_time - start_time}s') diff --git a/modules/python/generator/visp_python_bindgen/header.py b/modules/python/generator/visp_python_bindgen/header.py index 4b7ff3c1e6..1561b69d25 100644 --- a/modules/python/generator/visp_python_bindgen/header.py +++ b/modules/python/generator/visp_python_bindgen/header.py @@ -531,7 +531,11 @@ def add_method_doc_to_pyargs(method: types.Method, py_arg_strs: List[str]) -> Li continue field_type = get_type(field.type, owner_specs, header_env.mapping) - field_name_python = field.name.lstrip('m_') + field_name_python = field.name + prefix_member = 'm_' + if field_name_python.startswith(prefix_member): + field_name_python = field_name_python[len(prefix_member):] + logging.info(f'Found field in class/struct {name_cpp}: {field_type} {field.name}') def_str = 'def_' diff --git a/modules/python/test/test_numpy_array.py b/modules/python/test/test_numpy_array.py index 9ca1113c26..b0b4c1daef 100644 --- a/modules/python/test/test_numpy_array.py +++ b/modules/python/test/test_numpy_array.py @@ -131,3 +131,86 @@ def test_index_tuple_not_copy(): for i in range(2): for j in range(2): assert a[i, j] == 0.0 + +def test_setitem_2D_array(): + h,w = 50, 50 + a = ArrayDouble2D(h, w, 5) + + # 2D indexing (basic) + a[0, 0] = 5 + assert a[0, 0] == 5 + a[0, 0] = 20 + assert a[0, 0] == 20 + + # Replace a row + a[1] = 20 + for i in range(a.getCols()): + assert a[1, i] == 20 + + + # Replace a row + a[:] = 20 + for i in range(a.getRows()): + for j in range(a.getCols()): + assert a[i, j] == 20 + + # Replace rows with a slice + a[:] = 5 + a[::2] = 20 + for i in range(a.getRows()): + v = 5 if i % 2 == 1 else 20 + for j in range(a.getCols()): + assert a[i, j] == v + + a[:] = 5 + a[2:-2:2] = 20 + for i in range(a.getRows()): + v = 5 if i % 2 == 1 or i >= a.getRows() - 2 or i < 2 else 20 + for j in range(a.getCols()): + assert a[i, j] == v + + a[:, :] = 5 + for i in range(a.getRows()): + for j in range(a.getCols()): + assert a[i, j] == 5 + + # Indexing with two slices + a[2:-2:2, 3:-3] = 20 + for i in range(a.getRows()): + is_v = i >= 2 and i % 2 == 0 and i < a.getRows() - 2 + for j in range(a.getCols()): + is_vj = is_v and j >= 3 and j < a.getCols() - 3 + v = 20 if is_vj else 5 + assert a[i, j] == v + + + + # Negative step not supported + with pytest.raises(RuntimeError): + a[::-1] = 20 + with pytest.raises(RuntimeError): + a[:, ::-1] = 20 + + # Wrong start and end values + with pytest.raises(RuntimeError): + a[2:1] = 20 + with pytest.raises(RuntimeError): + a[:, 3:2] = 20 + + + a = ArrayDouble2D(h, w, 0.0) + single_row = np.ones((w, ), dtype=np.double) * 20 + + a[2] = single_row + assert not np.any(np.equal(a.numpy()[list(set(range(h)) - {2})], single_row)) + assert np.all(np.equal(a.numpy()[2], single_row)) + + a[:] = 0 + a[1:-2] = single_row + assert np.all(np.equal(a.numpy()[list(set(range(h)) - {0, h - 2, h - 1})], single_row)) + assert np.all(np.equal(a.numpy()[[0, h - 2, h - 1]], 0)) + + multi_rows = np.asarray([[i * w + j for j in range(w)] for i in range(h - 5)]) + + a[:-5] = multi_rows + assert np.all(np.equal(a.numpy()[:-5], multi_rows)) diff --git a/modules/python/test/test_numpy_image.py b/modules/python/test/test_numpy_image.py index 3dc2eff451..5c5fba449b 100644 --- a/modules/python/test/test_numpy_image.py +++ b/modules/python/test/test_numpy_image.py @@ -114,3 +114,83 @@ def test_np_array_replace_value(): vp_image[-vp_image.getHeight() - 1] with pytest.raises(RuntimeError): vp_image[0, -vp_image.getWidth() - 1] + + +def test_setitem_with_single_value(): + for test_dict in get_data_dicts(): + vp_image = test_dict['instance'] + # 2D indexing (basic) + vp_image[0, 0] = test_dict['base_value'] + assert vp_image[0, 0] == test_dict['base_value'] + vp_image[0, 0] = test_dict['value'] + assert vp_image[0, 0] == test_dict['value'] + + # Replace a row + vp_image[1] = test_dict['value'] + for i in range(vp_image.getCols()): + assert vp_image[1, i] == test_dict['value'] + + + # Replace a row + vp_image[:] = test_dict['value'] + for i in range(vp_image.getRows()): + for j in range(vp_image.getCols()): + assert vp_image[i, j] == test_dict['value'] + + # Replace rows with a slice + vp_image[:] = test_dict['base_value'] + vp_image[::2] = test_dict['value'] + for i in range(vp_image.getRows()): + v = test_dict['base_value'] if i % 2 == 1 else test_dict['value'] + for j in range(vp_image.getCols()): + assert vp_image[i, j] == v + + vp_image[:] = test_dict['base_value'] + vp_image[2:-2:2] = test_dict['value'] + for i in range(vp_image.getRows()): + v = test_dict['base_value'] if i % 2 == 1 or i >= vp_image.getRows() - 2 or i < 2 else test_dict['value'] + for j in range(vp_image.getCols()): + assert vp_image[i, j] == v + + vp_image[:, :] = test_dict['base_value'] + for i in range(vp_image.getRows()): + for j in range(vp_image.getCols()): + assert vp_image[i, j] == test_dict['base_value'] + + # Indexing with two slices + vp_image[2:-2:2, 3:-3] = test_dict['value'] + for i in range(vp_image.getRows()): + is_v = i >= 2 and i % 2 == 0 and i < vp_image.getRows() - 2 + for j in range(vp_image.getCols()): + is_vj = is_v and j >= 3 and j < vp_image.getCols() - 3 + v = test_dict['value'] if is_vj else test_dict['base_value'] + assert vp_image[i, j] == v + + + + # Negative step not supported + with pytest.raises(RuntimeError): + vp_image[::-1] = test_dict['value'] + with pytest.raises(RuntimeError): + vp_image[:, ::-1] = test_dict['value'] + + # Wrong start and end values + with pytest.raises(RuntimeError): + vp_image[2:1] = test_dict['value'] + with pytest.raises(RuntimeError): + vp_image[:, 3:2] = test_dict['value'] + + +def test_setitem_with_numpy_raw_image(): + h, w = 30, 20 + I = ImageGray(h, w, 0) + single_row = np.ones((w, ), dtype=np.uint8) * 255 + + I[2] = single_row + assert not np.any(np.equal(I.numpy()[list(set(range(h)) - {2})], single_row)) + assert np.all(np.equal(I.numpy()[2], single_row)) + + I[:] = 0 + I[1:-2] = single_row + assert np.all(np.equal(I.numpy()[list(set(range(h)) - {0, h - 2, h - 1})], single_row)) + assert np.all(np.equal(I.numpy()[[0, h - 2, h - 1]], 0)) diff --git a/modules/sensor/include/visp3/sensor/vpRealSense.h b/modules/sensor/include/visp3/sensor/vpRealSense.h index a040b9d508..4c8f6e3cb3 100644 --- a/modules/sensor/include/visp3/sensor/vpRealSense.h +++ b/modules/sensor/include/visp3/sensor/vpRealSense.h @@ -79,7 +79,7 @@ BEGIN_VISP_NAMESPACE file that allows to build sample-realsense.cpp that uses vpRealSense class. \code project(sample) - cmake_minimum_required(VERSION 3.5) + cmake_minimum_required(VERSION 3.10) find_package(VISP REQUIRED) include_directories(${VISP_INCLUDE_DIRS}) diff --git a/modules/sensor/include/visp3/sensor/vpRealSense2.h b/modules/sensor/include/visp3/sensor/vpRealSense2.h index 16c61e6972..2949093345 100644 --- a/modules/sensor/include/visp3/sensor/vpRealSense2.h +++ b/modules/sensor/include/visp3/sensor/vpRealSense2.h @@ -79,7 +79,7 @@ BEGIN_VISP_NAMESPACE uses vpRealSense2 class. \code - cmake_minimum_required(VERSION 3.5) + 10) project(sample) diff --git a/modules/tracker/dnn/CMakeLists.txt b/modules/tracker/dnn/CMakeLists.txt index fea2368b87..bede158701 100644 --- a/modules/tracker/dnn/CMakeLists.txt +++ b/modules/tracker/dnn/CMakeLists.txt @@ -34,8 +34,11 @@ ############################################################################# if(USE_NLOHMANN_JSON) + get_target_property(_inc_dirs "nlohmann_json::nlohmann_json" INTERFACE_INCLUDE_DIRECTORIES) + list(APPEND opt_incs ${_inc_dirs}) + vp_add_module(dnn_tracker visp_core) vp_glob_module_sources() - vp_module_include_directories() + vp_module_include_directories(${opt_incs}) vp_create_module() endif() diff --git a/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h b/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h index 6da9df8d28..60a8078eea 100644 --- a/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h +++ b/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h @@ -53,6 +53,10 @@ #include #include +#if defined(VISP_HAVE_NLOHMANN_JSON) +#include +#endif + BEGIN_VISP_NAMESPACE /*! * \class vpKltOpencv @@ -384,6 +388,11 @@ class VISP_EXPORT vpKltOpencv */ void suppressFeature(const int &index); +#ifdef VISP_HAVE_NLOHMANN_JSON + friend void to_json(nlohmann::json &j, const vpKltOpencv &array); + friend void from_json(const nlohmann::json &j, vpKltOpencv &array); +#endif + protected: cv::Mat m_gray; //!< Gray image cv::Mat m_prevGray; //!< Previous gray image @@ -402,6 +411,35 @@ class VISP_EXPORT vpKltOpencv long m_next_points_id; //!< Id for the newt keypoint bool m_initial_guess; //!< true when initial guess is provided }; + +#ifdef VISP_HAVE_NLOHMANN_JSON +inline void to_json(nlohmann::json &j, const vpKltOpencv &klt) +{ + j = nlohmann::json { + {"maxFeatures", klt.getMaxFeatures()}, + {"windowSize", klt.getWindowSize()}, + {"quality", klt.getQuality()}, + {"minDistance", klt.getMinDistance()}, + {"useHarris", klt.m_useHarrisDetector}, + {"harris", klt.getHarrisFreeParameter()}, + {"blockSize", klt.getBlockSize()}, + {"pyramidLevels", klt.getPyramidLevels()} + }; +} + +inline void from_json(const nlohmann::json &j, vpKltOpencv &klt) +{ + klt.setMaxFeatures(j.value("maxFeatures", 10000)); + klt.setWindowSize(j.value("windowSize", 5)); + klt.setQuality(j.value("quality", 0.01)); + klt.setMinDistance(j.value("minDistance", 5)); + klt.setUseHarris(j.value("useHarris", 1)); + klt.setHarrisFreeParameter(j.value("harris", 0.01)); + klt.setBlockSize(j.value("blockSize", 3)); + klt.setPyramidLevels(j.value("pyramidLevels", 3)); +} +#endif + END_VISP_NAMESPACE #endif #endif diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h index 6983bc0c33..1ba9be70b1 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h @@ -919,15 +919,7 @@ inline void to_json(nlohmann::json &j, const vpMbGenericTracker::TrackerWrapper //KLT tracker settings #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO) if (t.m_trackerType & vpMbGenericTracker::KLT_TRACKER) { - nlohmann::json klt = nlohmann::json { - {"maxFeatures", t.tracker.getMaxFeatures()}, - {"windowSize", t.tracker.getWindowSize()}, - {"quality", t.tracker.getQuality()}, - {"minDistance", t.tracker.getMinDistance()}, - {"harris", t.tracker.getHarrisFreeParameter()}, - {"blockSize", t.tracker.getBlockSize()}, - {"pyramidLevels", t.tracker.getPyramidLevels()} - }; + nlohmann::json klt = t.tracker; klt["maskBorder"] = t.maskBorder; j["klt"] = klt; } @@ -1030,14 +1022,7 @@ inline void from_json(const nlohmann::json &j, vpMbGenericTracker::TrackerWrappe #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO) if (t.m_trackerType & vpMbGenericTracker::KLT_TRACKER) { const nlohmann::json klt = j.at("klt"); - auto &ktrack = t.tracker; - ktrack.setMaxFeatures(klt.value("maxFeatures", 10000)); - ktrack.setWindowSize(klt.value("windowSize", 5)); - ktrack.setQuality(klt.value("quality", 0.01)); - ktrack.setMinDistance(klt.value("minDistance", 5)); - ktrack.setHarrisFreeParameter(klt.value("harris", 0.01)); - ktrack.setBlockSize(klt.value("blockSize", 3)); - ktrack.setPyramidLevels(klt.value("pyramidLevels", 3)); + t.tracker = klt; t.setMaskBorder(klt.value("maskBorder", t.maskBorder)); t.faces.getMbScanLineRenderer().setMaskBorder(t.maskBorder); } diff --git a/modules/tracker/rbt/CMakeLists.txt b/modules/tracker/rbt/CMakeLists.txt new file mode 100644 index 0000000000..174fd255db --- /dev/null +++ b/modules/tracker/rbt/CMakeLists.txt @@ -0,0 +1,75 @@ +############################################################################# +# +# ViSP, open source Visual Servoing Platform software. +# Copyright (C) 2005 - 2023 by Inria. All rights reserved. +# +# This software is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# See the file LICENSE.txt at the root directory of this source +# distribution for additional information about the GNU GPL. +# +# For using ViSP with software that can not be combined with the GNU +# GPL, please contact Inria about acquiring a ViSP Professional +# Edition License. +# +# See https://visp.inria.fr for more information. +# +# This software was developed at: +# Inria Rennes - Bretagne Atlantique +# Campus Universitaire de Beaulieu +# 35042 Rennes Cedex +# France +# +# If you have questions regarding the use of this file, please contact +# Inria at visp@inria.fr +# +# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE +# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. +# +# Description: +# ViSP configuration file. +# +############################################################################# + +# Add optional 3rd parties +set(opt_incs "") +set(opt_libs "") +set(opt_libs_private "") + +if(NOT USE_PANDA3D OR VISP_CXX_STANDARD LESS VISP_CXX_STANDARD_11) + return() +endif() + +if(USE_NLOHMANN_JSON) + get_target_property(_inc_dirs "nlohmann_json::nlohmann_json" INTERFACE_INCLUDE_DIRECTORIES) + list(APPEND opt_incs ${_inc_dirs}) +endif() + +if(WITH_SIMDLIB) + # Simd lib is private + include_directories(${SIMDLIB_INCLUDE_DIRS}) + list(APPEND opt_libs_private ${SIMDLIB_LIBRARIES}) +endif() + +vp_add_module(rbt visp_vision visp_core visp_me visp_visual_features visp_ar OPTIONAL visp_klt visp_gui PRIVATE_OPTIONAL ${opt_libs_private}) +vp_glob_module_sources() + +vp_module_include_directories(${opt_incs}) +vp_create_module(${opt_libs}) + +set(opt_test_incs "") +set(opt_test_libs "") + +if(WITH_CATCH2) + # catch2 is private + list(APPEND opt_test_incs ${CATCH2_INCLUDE_DIRS}) + list(APPEND opt_test_libs ${CATCH2_LIBRARIES}) +endif() + +vp_add_tests(DEPENDS_ON visp_core visp_gui visp_io PRIVATE_INCLUDE_DIRS ${opt_test_incs} PRIVATE_LIBRARIES ${opt_test_libs}) + +if(VISP_DATASET_FOUND) + +endif() diff --git a/modules/tracker/rbt/include/visp3/rbt/vpColorHistogram.h b/modules/tracker/rbt/include/visp3/rbt/vpColorHistogram.h new file mode 100644 index 0000000000..a390b5ad0d --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpColorHistogram.h @@ -0,0 +1,123 @@ +/* + * 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. + */ + +/*! + \file vpColorHistogram.h + \brief Color histogram representation +*/ +#ifndef VP_COLOR_HISTOGRAM_H +#define VP_COLOR_HISTOGRAM_H + +#include +#include + +#include + +BEGIN_VISP_NAMESPACE + +template +class vpImage; +class vpRGBa; +class vpRect; + + + +class VISP_EXPORT vpColorHistogram +{ +public: + + class Builder + { + public: + Builder(unsigned int N) : m_counts(N *N *N, 0), m_N(N), m_binSize(256 / N) { } + inline void add(const vpRGBa &color) + { + unsigned int index = (color.R / m_binSize) * (m_N * m_N) + (color.G / m_binSize) * m_N + (color.B / m_binSize); + ++m_counts[index]; + } + void build(vpColorHistogram &histogram); + private: + std::vector m_counts; + unsigned int m_N, m_binSize; + }; + + vpColorHistogram(); + vpColorHistogram(unsigned int N); + + /** + * \brief Change the number of bins per color component that the histogram has + * After calling this method, the histogram will be reset and the values will not be kept. + * \param N the number of bins per RGB component: the histogram will have N^3 bins in total. N should be a power of 2 between 1 and 128 + */ + void setBinNumber(unsigned int N); + + unsigned int getBinNumber() const { return m_N; } + + unsigned int getNumPixels() const { return m_numPixels; } + + void build(const vpImage &image, const vpImage &mask); + + void build(const std::vector &counts); + + void merge(const vpColorHistogram &other, float alpha); + + void computeProbas(const vpImage &image, vpImage &proba) const; + void computeProbas(const vpImage &image, vpImage &proba, const vpRect &bb) const; + + + inline unsigned int colorToIndex(const vpRGBa &p) const + { + return (p.R / m_binSize) * (m_N * m_N) + (p.G / m_binSize) * m_N + (p.B / m_binSize); + } + + inline double probability(const vpRGBa &color) const + { + return m_probas[colorToIndex(color)]; + } + + double kl(const vpColorHistogram &other) const; + + double jsd(const vpColorHistogram &other) const; + + double hellinger(const vpColorHistogram &other) const; + + static void computeSplitHistograms(const vpImage &image, const vpImage &mask, vpColorHistogram &inMask, vpColorHistogram &outsideMask); + static void computeSplitHistograms(const vpImage &image, const vpImage &mask, const vpRect &bbInside, vpColorHistogram &insideMask, vpColorHistogram &outsideMask); + +private: + unsigned int m_N; + unsigned int m_binSize; + std::vector m_probas; + unsigned int m_numPixels; +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpColorHistogramMask.h b/modules/tracker/rbt/include/visp3/rbt/vpColorHistogramMask.h new file mode 100644 index 0000000000..84b46088db --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpColorHistogramMask.h @@ -0,0 +1,131 @@ +/* + * 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. + */ + +/*! + \file vpColorHistogramMask.h + \brief Object mask estimation through global foreground/background color histogram representations +*/ +#ifndef VP_COLOR_HISTOGRAM_MASK_H +#define VP_COLOR_HISTOGRAM_MASK_H + +#include +#include +#include +#include +#include + +#ifdef VISP_HAVE_NLOHMANN_JSON +#include VISP_NLOHMANN_JSON(json_fwd.hpp) +#endif + +BEGIN_VISP_NAMESPACE + +class vpRBFeatureTrackerInput; + +/** + * \brief A color histogram based segmentation algorithm. + * \ingroup group_rbt_mask +*/ +class VISP_EXPORT vpColorHistogramMask : public vpObjectMask +{ +public: + vpColorHistogramMask(); + virtual ~vpColorHistogramMask() = default; + + void updateMask(const vpRBFeatureTrackerInput &frame, + const vpRBFeatureTrackerInput &previousFrame, + vpImage &mask) VP_OVERRIDE; + + /** + * \name Histogram settings + * @{ + */ + void setBinNumber(unsigned int N) + { + m_histBackground.setBinNumber(N); + m_histBackgroundFrame.setBinNumber(N); + m_histObject.setBinNumber(N); + m_histObjectFrame.setBinNumber(N); + } + + float getDepthErrorTolerance() const { return m_depthErrorTolerance; } + void setDepthErrorTolerance(float errorMax) + { + if (errorMax < 0.f) { + throw vpException(vpException::badValue, "Depth error tolerance in histogram computation should be > 0"); + } + m_depthErrorTolerance = errorMax; + } + + float getObjectUpdateRate() const { return m_objectUpdateRate; } + void setObjectUpdateRate(float updateRate) + { + if (updateRate < 0.f || updateRate > 1.f) { + throw vpException(vpException::badValue, "Histogram update rate should be between 0 and 1 (included)"); + } + m_objectUpdateRate = updateRate; + } + + float getBackgroundUpdateRate() const { return m_backgroundUpdateRate; } + void setBackgroundUpdateRate(float updateRate) + { + if (updateRate < 0.f || updateRate > 1.f) { + throw vpException(vpException::badValue, "Histogram update rate should be between 0 and 1 (included)"); + } + m_backgroundUpdateRate = updateRate; + } + + bool isComputedOnlyOnBoundingBox() const { return m_computeOnBBOnly; } + void setComputeOnlyOnBoundingBox(bool bbOnly) + { + m_computeOnBBOnly = bbOnly; + } + /** + * @} + */ + +#if defined(VISP_HAVE_NLOHMANN_JSON) + void loadJsonConfiguration(const nlohmann::json &json) VP_OVERRIDE; +#endif + +private: + vpColorHistogram m_histObject, m_histBackground, m_histObjectFrame, m_histBackgroundFrame; + float m_depthErrorTolerance; + float m_objectUpdateRate, m_backgroundUpdateRate; + + vpImage m_mask; + vpImage m_probaObject, m_probaBackground; + + bool m_computeOnBBOnly; +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpDynamicFactory.h b/modules/tracker/rbt/include/visp3/rbt/vpDynamicFactory.h new file mode 100644 index 0000000000..6a5dae8669 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpDynamicFactory.h @@ -0,0 +1,111 @@ +/* + * 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. + */ + +/*! + \file vpDynamicFactory.h + \brief Factory-type class that allows for the dynamic registration of subclasses +*/ +#ifndef VP_DYNAMIC_FACTORY_H +#define VP_DYNAMIC_FACTORY_H + +#include +#include + +#if defined(VISP_HAVE_NLOHMANN_JSON) +#include VISP_NLOHMANN_JSON(json.hpp) +#endif + +#include +#include + +BEGIN_VISP_NAMESPACE +template +class VISP_EXPORT vpDynamicFactory +{ +public: +#if defined(VISP_HAVE_NLOHMANN_JSON) + void registerType(const std::string &key, const std::function(const nlohmann::json &)> &function) + { + if (m_jsonBuilders.find(key) != m_jsonBuilders.end() || m_jsonRawBuilders.find(key) != m_jsonRawBuilders.end()) { + throw vpException(vpException::badValue, "Type %s was already registered in the factory", key.c_str()); + } + m_jsonBuilders[key] = function; + } + + void registerTypeRaw(const std::string &key, const std::function(const std::string &)> function) + { + if (m_jsonBuilders.find(key) != m_jsonBuilders.end() || m_jsonRawBuilders.find(key) != m_jsonRawBuilders.end()) { + throw vpException(vpException::badValue, "Type %s was already registered in the factory", key.c_str()); + } + + m_jsonRawBuilders.insert({ key, function }); + // m_jsonRawBuilders[key] = [](const std::string &s) -> std::shared_ptr { + + } + + std::shared_ptr buildFromJson(const nlohmann::json &j) + { + const std::string key = m_keyFinder(j); + + if (m_jsonBuilders.find(key) != m_jsonBuilders.end()) { + return m_jsonBuilders[key](j); + } + + else if (m_jsonRawBuilders.find(key) != m_jsonRawBuilders.end()) { + return m_jsonRawBuilders[key](j.dump()); + } + else { + return nullptr; + } + } + + + void setJsonKeyFinder(const std::function &finderFn) + { + m_keyFinder = finderFn; + } +#endif + + virtual ~vpDynamicFactory() { } + +protected: + + vpDynamicFactory() = default; +#if defined(VISP_HAVE_NLOHMANN_JSON) + std::map(const nlohmann::json &)>> m_jsonBuilders; + std::map(const std::string &)>> m_jsonRawBuilders; + + std::function m_keyFinder; //! Function to retrieve the key from a json object +#endif +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpObjectCentricRenderer.h b/modules/tracker/rbt/include/visp3/rbt/vpObjectCentricRenderer.h new file mode 100644 index 0000000000..58fc13f18b --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpObjectCentricRenderer.h @@ -0,0 +1,123 @@ +/* + * 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. + */ + +/*! + \file vpObjectCentricRenderer.h + \brief Single object focused renderer +*/ +#ifndef VP_OBJECT_CENTRIC_RENDERER_H +#define VP_OBJECT_CENTRIC_RENDERER_H + +#include + +#if defined(VISP_HAVE_PANDA3D) + +#include +#include +#include + +BEGIN_VISP_NAMESPACE + +/*! + \brief Single object focused renderer + \ingroup group_rbt_rendering +*/ +class VISP_EXPORT vpObjectCentricRenderer : public vpPanda3DRendererSet +{ +public: + vpObjectCentricRenderer(const vpPanda3DRenderParameters &renderParameters); + + virtual void setRenderParameters(const vpPanda3DRenderParameters ¶ms) VP_OVERRIDE + { + vpPanda3DRendererSet::setRenderParameters(params); + } + + vpRect getBoundingBox() const { return m_bb; } + + void setFocusedObject(const std::string &focused) + { + m_focusedObject = focused; + m_shouldComputeBBPoints = true; + } + + void beforeFrameRendered() VP_OVERRIDE; + + void computeBoundingBox3DPoints(); + void computeClipping(float &nearV, float &farV); + + std::vector getBoundingBox3D() + { + if (m_shouldComputeBBPoints) { + computeBoundingBox3DPoints(); + m_shouldComputeBBPoints = false; + } + return m_bb3DPoints; + } + + vpRect computeBoundingBox(); + + template + void placeRenderInto(const vpImage &render, vpImage &target, const T &clearValue) + { + if (!m_enableCrop) { + target = render; + } + else { + const unsigned h = m_renderParameters.getImageHeight(); + const unsigned w = m_renderParameters.getImageWidth(); + const unsigned top = static_cast(std::max(0.0, m_bb.getTop())); + const unsigned left = static_cast(std::max(0.0, m_bb.getLeft())); + const unsigned bottom = static_cast(std::min(static_cast(h), m_bb.getBottom())); + const unsigned right = static_cast(std::min(static_cast(w), m_bb.getRight())); + + target.resize(h, w, clearValue); + for (unsigned int i = top; i < bottom; ++i) { + memcpy(target.bitmap + i * w + left, render[i - top], (right - left) * sizeof(T)); + // for (unsigned int j = left; j < right; ++j) { + // target[i][j] = render[i - unsigned(m_bb.getTop())][j - unsigned(m_bb.getLeft())]; + // } + } + + } + } + +private: + bool m_enableCrop; + std::string m_focusedObject; + vpRect m_bb; + std::vector m_bb3DPoints; + bool m_shouldComputeBBPoints; + vpPanda3DRenderParameters m_subRenderParams; +}; + +END_VISP_NAMESPACE + +#endif +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpObjectMask.h b/modules/tracker/rbt/include/visp3/rbt/vpObjectMask.h new file mode 100644 index 0000000000..c0dc0ad757 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpObjectMask.h @@ -0,0 +1,71 @@ +/* + * 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. + */ + +/*! + \file vpObjectMask.h + \brief Object mask segmentation for the render-based tracker +*/ +#ifndef VP_OBJECT_MASK_H +#define VP_OBJECT_MASK_H + +#include + +#if defined(VISP_HAVE_NLOHMANN_JSON) +#include VISP_NLOHMANN_JSON(json_fwd.hpp) +#endif + +BEGIN_VISP_NAMESPACE + +template +class vpImage; +class vpRBFeatureTrackerInput; + +/** + * \brief + * + * \ingroup group_rbt_mask +*/ +class VISP_EXPORT vpObjectMask +{ +public: + virtual void updateMask(const vpRBFeatureTrackerInput &frame, + const vpRBFeatureTrackerInput &previousFrame, + vpImage &mask) = 0; + + virtual void display(const vpImage &mask, vpImage &Imask) const; + +#if defined(VISP_HAVE_NLOHMANN_JSON) + virtual void loadJsonConfiguration(const nlohmann::json &j) = 0; +#endif +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpObjectMaskFactory.h b/modules/tracker/rbt/include/visp3/rbt/vpObjectMaskFactory.h new file mode 100644 index 0000000000..67321f3652 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpObjectMaskFactory.h @@ -0,0 +1,63 @@ +/* + * 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. + */ + +/*! + \file vpObjectMaskFactory.h + \brief Factory for algorithms that can compute object masks for the render based tracker +*/ +#ifndef VP_OBJECT_MASK_FACTORY_H +#define VP_OBJECT_MASK_FACTORY_H + +#include +#include +#include + +BEGIN_VISP_NAMESPACE +/** +*\brief A factory that can be used to create Object segmentation algorithms from JSON data. +* +*\ingroup group_rbt_mask +*/ +class VISP_EXPORT vpObjectMaskFactory : public vpDynamicFactory +{ +private: + vpObjectMaskFactory(); + +public: + static vpObjectMaskFactory &getFactory() + { + static vpObjectMaskFactory factory; + return factory; + } +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpPanda3DDepthFilters.h b/modules/tracker/rbt/include/visp3/rbt/vpPanda3DDepthFilters.h new file mode 100644 index 0000000000..a707e9467c --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpPanda3DDepthFilters.h @@ -0,0 +1,93 @@ +/* + * 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. + */ + +/*! + \file vpPanda3DDepthFilters.h + \brief Custom shaders for depth silhouette extraction +*/ +#ifndef VP_PANDA3D_DEPTH_FILTERS_H +#define VP_PANDA3D_DEPTH_FILTERS_H + +#include + +#if defined(VISP_HAVE_PANDA3D) + +#include +#include + +BEGIN_VISP_NAMESPACE +/** + * + * \ingroup group_rbt_rendering + * \brief +*/ +class VISP_EXPORT vpPanda3DDepthGaussianBlur : public vpPanda3DPostProcessFilter +{ +public: + vpPanda3DDepthGaussianBlur(const std::string &name, std::shared_ptr inputRenderer, bool isOutput); + FrameBufferProperties getBufferProperties() const VP_OVERRIDE; + void getRender(vpImage &I) const; + +private: + static const std::string FRAGMENT_SHADER; +}; + +/** + * \ingroup group_rbt_rendering + * \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 vpPanda3DDepthCannyFilter : public vpPanda3DPostProcessFilter +{ +public: + vpPanda3DDepthCannyFilter(const std::string &name, std::shared_ptr inputRenderer, bool isOutput, float edgeThreshold); + FrameBufferProperties getBufferProperties() const VP_OVERRIDE; + void getRender(vpImage &I, vpImage &valid) const; + void getRender(vpImage &I, vpImage &valid, const vpRect &bb, unsigned int h, unsigned w) const; + + void setEdgeThreshold(float edgeThreshold); + +protected: + void setupScene() VP_OVERRIDE; + +private: + static const std::string FRAGMENT_SHADER; + float m_edgeThreshold; +}; + +END_VISP_NAMESPACE + +#endif +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h new file mode 100644 index 0000000000..07e1e9c3aa --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBDenseDepthTracker.h @@ -0,0 +1,202 @@ +/* + * 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. + */ + +/*! + \file vpRBDenseDepthTracker.h + \brief Dense depth features for render-based tracking +*/ + +#ifndef VP_RB_DENSE_DEPTH_TRACKER_H +#define VP_RB_DENSE_DEPTH_TRACKER_H + +#include +#include +#include +#include +#include +#include +#include + +// #if defined(VISP_HAVE_SIMDLIB) +// #include +// #endif +#include + +#include +#include +#include + +#if defined(VISP_HAVE_NLOHMANN_JSON) +#include VISP_NLOHMANN_JSON(json.hpp) +#endif + +BEGIN_VISP_NAMESPACE +/** + * @brief A tracker based on dense depth point-plane alignment. + * + * \ingroup group_rbt_trackers + * +*/ +class VISP_EXPORT vpRBDenseDepthTracker : public vpRBFeatureTracker +{ +public: + + vpRBDenseDepthTracker() : vpRBFeatureTracker(), m_step(2), m_useMask(false), m_minMaskConfidence(0.f) { } + + virtual ~vpRBDenseDepthTracker() = default; + + bool requiresRGB() const VP_OVERRIDE { return false; } + bool requiresDepth() const VP_OVERRIDE { return true; } + bool requiresSilhouetteCandidates() const VP_OVERRIDE { return false; } + + /** + * \name Settings + * @{ + */ + unsigned int getStep() const { return m_step; } + void setStep(unsigned int step) + { + if (step == 0) { + throw vpException(vpException::badValue, "Step should be greater than 0"); + } + m_step = step; + } + + /** + * \brief Returns whether the tracking algorithm should filter out points that are unlikely to be on the object according to the mask. + * If the mask is not computed beforehand, then it has no effect + */ + bool shouldUseMask() const { return m_useMask; } + void setShouldUseMask(bool useMask) { m_useMask = useMask; } + + /** + * \brief Returns the minimum mask confidence that a pixel linked to depth point should have if it should be kept during tracking. + * + * This value is between 0 and 1 + */ + float getMinimumMaskConfidence() const { return m_minMaskConfidence; } + void setMinimumMaskConfidence(float confidence) + { + if (confidence > 1.f || confidence < 0.f) { + throw vpException(vpException::badValue, "Mask confidence should be between 0 and 1"); + } + m_minMaskConfidence = confidence; + } + + /** + * \name Settings + * @} + */ + + /** + * @brief Method called when starting a tracking iteration + */ + void onTrackingIterStart() VP_OVERRIDE { } + void onTrackingIterEnd() VP_OVERRIDE { } + + double getVVSTrackerWeight() const VP_OVERRIDE { return m_userVvsWeight / (m_error.size()); } + + void extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; + void trackFeatures(const vpRBFeatureTrackerInput &/*frame*/, const vpRBFeatureTrackerInput &/*previousFrame*/, const vpHomogeneousMatrix &/*cMo*/) VP_OVERRIDE { } + void initVVS(const vpRBFeatureTrackerInput &/*frame*/, const vpRBFeatureTrackerInput &/*previousFrame*/, const vpHomogeneousMatrix &/*cMo*/) VP_OVERRIDE { } + void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) VP_OVERRIDE; + + void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth) const VP_OVERRIDE; + + struct vpDepthPoint + { + vpDepthPoint() : currentPoint(3), cameraNormal(3), objectNormal(3) + { } + + inline void update(const vpHomogeneousMatrix &cMo, const vpRotationMatrix &cRo) + { + oP.changeFrame(cMo); + oP.projection(); + cameraNormal = cRo * objectNormal; + // f.buildFrom(oP.get_x(), oP.get_y(), oP.get_Z(), log(oP.get_Z() / currentPoint.get_Z())); + } + + inline void error(vpColVector &e, unsigned i) const + { + double D = -((cameraNormal[0] * oP.get_X()) + (cameraNormal[1] * oP.get_Y()) + (cameraNormal[2] * oP.get_Z())); + double projNormal = cameraNormal[0] * currentPoint[0] + cameraNormal[1] * currentPoint[1] + cameraNormal[2] * currentPoint[2]; + + e[i] = D + projNormal; + //e[i] = f.get_LogZoverZstar(); + } + + inline void interaction(vpMatrix &L, unsigned i) + { + const double X = currentPoint[0], Y = currentPoint[1], Z = currentPoint[2]; + const double nx = cameraNormal[0], ny = cameraNormal[1], nz = cameraNormal[2]; + L[i][0] = nx; + L[i][1] = ny; + L[i][2] = nz; + L[i][3] = nz * Y - ny * Z; + L[i][4] = nx * Z - nz * X; + L[i][5] = ny * X - nx * Y; + // vpMatrix LL = f.interaction(); + // for (unsigned int j = 0; j < 6; ++j) { + // L[i][j] = LL[0][j]; + // } + } + + public: + vpPoint oP; + vpColVector currentPoint; + vpColVector cameraNormal; + vpColVector objectNormal; + vpImagePoint pixelPos; + //vpFeatureDepth f; + }; + +#if defined(VISP_HAVE_NLOHMANN_JSON) + virtual void loadJsonConfiguration(const nlohmann::json &j) VP_OVERRIDE + { + vpRBFeatureTracker::loadJsonConfiguration(j); + setStep(j.value("step", m_step)); + setShouldUseMask(j.value("useMask", m_useMask)); + setMinimumMaskConfidence(j.value("minMaskConfidence", m_minMaskConfidence)); + } + +#endif + +protected: + + std::vector m_depthPoints; + vpRobust m_robust; + unsigned int m_step; + bool m_useMask; + float m_minMaskConfidence; +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetector.h b/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetector.h new file mode 100644 index 0000000000..949744c8cd --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetector.h @@ -0,0 +1,110 @@ +/* + * 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. + */ + +/*! + \file vpRBDriftDetector.h + \brief Base class for drift/divergence detection algorithms for the render-based tracker +*/ +#ifndef VP_RB_DRIFT_DETECTOR_H +#define VP_RB_DRIFT_DETECTOR_H + +#include + +#if defined(VISP_HAVE_NLOHMANN_JSON) +#include VISP_NLOHMANN_JSON(json_fwd.hpp) +#endif + + +BEGIN_VISP_NAMESPACE + +class vpRBFeatureTrackerInput; +class vpHomogeneousMatrix; +class vpRGBa; +template class vpImage; + +/** + * \brief Base interface for algorithms that should detect tracking drift for the render-based tracker. + * + * In the tracking loop, these algorithms should be used as follows: + * - Perform a tracking step, estimating a new object pose + * - Call vpRBDriftDetector::update to update the drift detection parameters. + * - use vpRBDriftDetector::hasDiverged to detect the drift, or vpRBDriftDetector::getScore to use the estimated tracking reliability. + * + * \ingroup group_rbt_drift +*/ +class VISP_EXPORT vpRBDriftDetector +{ +public: + vpRBDriftDetector() = default; + + virtual ~vpRBDriftDetector() = default; + + /** + * \brief Update the algorithm after a new tracking step. + * + * \param previousFrame The previous frame data: contains the input images at t-1 (linked to cprevTo) and the renders at t-2. May be empty for the first iteration + * \param frame The current frame data: contains the input images at time t (linked to the newly estimated cTo) and the renders at t-1 (linked to cprevTo) + * \param cTo the newly estimated object pose in the camera frame + * \param cprevTo the previously estimated object pose in the camera frame + */ + virtual void update(const vpRBFeatureTrackerInput &previousFrame, const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &cprevTo) = 0; + + /** + * \brief Get the estimated tracking reliability. + * A high score should mean that the tracking is reliable. + * Different algorithms may use different value ranges. + * + * \return The estimated tracking accuracy + */ + virtual double getScore() const = 0; + + /** + * \brief Returns whether the tracking has diverged and should be reinitialized. + * This function should be called after update. + * + */ + virtual bool hasDiverged() const = 0; + + /** + * \brief Displays the information used for drift detection. + * + * \param I the image in which to display the information + */ + virtual void display(const vpImage &I) = 0; + +#if defined(VISP_HAVE_NLOHMANN_JSON) + virtual void loadJsonConfiguration(const nlohmann::json &) = 0; +#endif + +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetectorFactory.h b/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetectorFactory.h new file mode 100644 index 0000000000..4b3c933582 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBDriftDetectorFactory.h @@ -0,0 +1,64 @@ +/* + * 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. + */ + +/*! + \file vpRBDriftDetectorFactory.h + \brief Drift detection algorithm factory +*/ +#ifndef VP_RB_DRIFT_DETECTOR_FACTORY_H +#define VP_RB_DRIFT_DETECTOR_FACTORY_H + +#include +#include +#include + +BEGIN_VISP_NAMESPACE + +/** + * \brief A factory that can be used to instanciate drift detection algorithms from JSON data. + * + * \ingroup group_rbt_drift +*/ +class VISP_EXPORT vpRBDriftDetectorFactory : public vpDynamicFactory +{ +private: + vpRBDriftDetectorFactory(); + +public: + static vpRBDriftDetectorFactory &getFactory() + { + static vpRBDriftDetectorFactory factory; + return factory; + } +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h new file mode 100644 index 0000000000..dd7a02a7c3 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTracker.h @@ -0,0 +1,244 @@ +/* + * 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. + */ + +/*! + \file vpRBFeatureTracker.h + \brief Base class for features that can be tracked using vpRBTracker +*/ +#ifndef VP_RB_FEATURE_TRACKER_H +#define VP_RB_FEATURE_TRACKER_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#if defined(VISP_HAVE_NLOHMANN_JSON) +#include VISP_NLOHMANN_JSON(json.hpp) +#endif + + +BEGIN_VISP_NAMESPACE +/** + * \brief A base class for all features that can be used and tracked in the vpRBTracker + * + * \ingroup group_rbt_trackers +*/ +class VISP_EXPORT vpRBFeatureTracker +{ +public: + + vpRBFeatureTracker(); + + /** + * \brief Return the type of feature that is used by this tracker + * + * \return vpRBFeatureType + */ + + /** + * \brief Get the number of features used to compute the pose update + * + */ + unsigned getNumFeatures() const { return m_numFeatures; } + + /** + * \name Required inputs + * @{ + */ + + /** + * \brief Whether this tracker requires RGB image to extract features + * + * \return true if the tracker requires an RGB image + * \return false otherwise + */ + virtual bool requiresRGB() const = 0; + + /** + * \brief Whether this tracker requires depth image to extract features + * + */ + virtual bool requiresDepth() const = 0; + + /** + * \brief Whether this tracker requires Silhouette candidates + */ + virtual bool requiresSilhouetteCandidates() const = 0; + /** + * @} + */ + + /** + * \name Core Tracking methods + * @{ + */ + + /** + * \brief Method called when starting a tracking iteration + * + */ + virtual void onTrackingIterStart() = 0; + + /** + * \brief Method called after the tracking iteration has finished + * + */ + virtual void onTrackingIterEnd() = 0; + + /** + * \brief Extract features from the frame data and the current pose estimate + * + */ + virtual void extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) = 0; + + /** + * \brief Track the features + */ + virtual void trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) = 0; + + virtual void initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) = 0; + virtual void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) = 0; + + /** + * @} + */ + + /** + * \name Display + * @{ + */ + + bool featuresShouldBeDisplayed() const { return m_enableDisplay; } + void setFeaturesShouldBeDisplayed(bool enableDisplay) { m_enableDisplay = enableDisplay; } + + virtual void display( + const vpCameraParameters &cam, + const vpImage &I, + const vpImage &IRGB, + const vpImage &depth) const = 0; + + /** + * @} + */ + + + /** + * \name Covariance computation + * @{ + */ + /** + * \brief Retrieve the 6 x 6 pose covariance matrix, computed from the weights associated to each feature. + * + * The updateCovariance method should have been called before + */ + virtual const vpMatrix getCovariance() const { return m_cov; } + /** + * \brief Update the covariance matrix + * + * \param lambda the visual servoing gain + */ + virtual void updateCovariance(const double lambda); + /** + * @} + */ + + /** + * Returns whether the tracker is considered as having converged to the desired pose. + */ + bool vvsHasConverged() const { return m_vvsConverged; } + + /** + * \brief Get the importance of this tracker in the optimization step. + * The default computation is the following: + * \f$ w / N \f$, where \f$ w\f$ is the weight defined by setTrackerWeight, and \f$ N \f$ is the number of features. + */ + virtual double getVVSTrackerWeight() const { return m_userVvsWeight / m_numFeatures; } + void setTrackerWeight(double weight) { m_userVvsWeight = weight; } + + /** + * \brief Get the left-side term of the Gauss-Newton optimization term + */ + virtual vpMatrix getLTL() const { return m_LTL; } + + /** + * \brief Get the right-side term of the Gauss-Newton optimization term + */ + virtual vpColVector getLTR() const { return m_LTR; } + + /** + * \brief Get a weighted version of the error vector. + * This should not include the userVVSWeight, but may include reweighting to remove outliers, occlusions, etc. + */ + const vpColVector &getWeightedError() const { return m_weighted_error; } + +#if defined(VISP_HAVE_NLOHMANN_JSON) + virtual void loadJsonConfiguration(const nlohmann::json &j) + { + m_userVvsWeight = j.at("weight"); + m_enableDisplay = j.value("display", m_enableDisplay); + } +#endif + + static void computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR); + static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &b, const vpMatrix &W); + +protected: + + + vpMatrix m_L; //! Error jacobian (In VS terms, the interaction matrix) + vpMatrix m_LTL; //! Left side of the Gauss newton minimization + vpColVector m_LTR; //! Right side of the Gauss Newton minimization + vpMatrix m_cov; //! Covariance matrix + vpColVector m_covWeightDiag; + + vpColVector m_error; //! Raw VS Error vector + vpColVector m_weighted_error; //! Weighted VS error + vpColVector m_weights; //! Error weights + + + unsigned m_numFeatures; //! Number of considered features + double m_userVvsWeight; //! User-defined weight for this specific type of feature + + bool m_vvsConverged; //! Whether VVS has converged, should be updated every VVS iteration + + bool m_enableDisplay; //! Whether the tracked features should be displayed. + +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerFactory.h b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerFactory.h new file mode 100644 index 0000000000..7e4cc59f50 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerFactory.h @@ -0,0 +1,65 @@ +/* + * 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. + */ + +/*! + \file vpRBFeatureTrackerFactory.h + \brief Factory for trackable features +*/ +#ifndef VP_RB_FEATURE_TRACKER_FACTORY_H +#define VP_RB_FEATURE_TRACKER_FACTORY_H + +#include +#include +#include + +BEGIN_VISP_NAMESPACE +/** + * \brief A factory to instantiate feature trackers from JSON data + * + * \ingroup group_rbt_trackers +*/ +class VISP_EXPORT vpRBFeatureTrackerFactory : public vpDynamicFactory +{ +private: + vpRBFeatureTrackerFactory(); + +public: + virtual ~vpRBFeatureTrackerFactory() = default; + static vpRBFeatureTrackerFactory &getFactory() + { + static vpRBFeatureTrackerFactory factory; + return factory; + } + +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerInput.h b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerInput.h new file mode 100644 index 0000000000..43e25e4a0a --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBFeatureTrackerInput.h @@ -0,0 +1,165 @@ +/* + * 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. + */ + +/*! + \file vpRBFeatureTrackerInput.h + \brief Processable frame for render-based tracking. This includes input images as well as renders +*/ +#ifndef VP_RB_FEATURE_TRACKER_INPUT_H +#define VP_RB_FEATURE_TRACKER_INPUT_H + +#include +#include +#include +#include + +#include + +BEGIN_VISP_NAMESPACE + + +/** + * \brief Render data storage + * + * \ingroup group_rbt_core +*/ +struct VISP_EXPORT vpRBRenderData +{ + vpImage normals; //! Image containing the per-pixel normal vector (RGB, in object space) + vpImage depth; + vpImage color; + vpImage silhouetteCanny; //! Image containing the orientation of the gradients + vpImage isSilhouette; //! Binary image indicating whether a given pixel is part of the silhouette + double zNear, zFar; // clipping values + vpRect boundingBox; + vpHomogeneousMatrix cMo; //! Pose of the object in the camera frame for which the renders were generated. + + vpRBRenderData() : zNear(0.0), zFar(0.0), boundingBox() { } + + vpRBRenderData(const vpRBRenderData &other) + { + *this = other; + } + + vpRBRenderData(vpRBRenderData &&other) + { + *this = std::move(other); + } + + vpRBRenderData &operator=(const vpRBRenderData &o) + { + normals = o.normals; + depth = o.depth; + color = o.color; + silhouetteCanny = o.silhouetteCanny; + isSilhouette = o.isSilhouette; + zNear = o.zNear; + zFar = o.zFar; + boundingBox = o.boundingBox; + cMo = o.cMo; + return *this; + } + + vpRBRenderData &operator=(vpRBRenderData &&o) + { + normals = std::move(o.normals); + depth = std::move(o.depth); + color = std::move(o.color); + silhouetteCanny = std::move(o.silhouetteCanny); + isSilhouette = std::move(o.isSilhouette); + zNear = std::move(o.zNear); + zFar = std::move(o.zFar); + boundingBox = std::move(o.boundingBox); + cMo = std::move(o.cMo); + return *this; + } +}; + +/** + * \brief All the data related to a single tracking frame. + * This contains both the input data (from a real camera/outside source) and renders from Panda. + * + * \ingroup group_rbt_core + */ +class VISP_EXPORT vpRBFeatureTrackerInput +{ +public: + vpImage I; //! Image luminance + vpImage IRGB; //! RGB image, 0 sized if RGB is not available + vpImage depth; //! depth image, 0 sized if depth is not available + vpImage mask; + std::vector silhouettePoints; + vpCameraParameters cam; //! camera parameters + vpRBRenderData renders; + + bool hasDepth() const { return depth.getSize() > 0; } + bool hasMask() const { return mask.getSize() > 0; } + bool hasColorImage() const { return IRGB.getSize() > 0; } + + vpRBFeatureTrackerInput() = default; + + vpRBFeatureTrackerInput &operator=(const vpRBFeatureTrackerInput &o) + { + I = o.I; + IRGB = o.IRGB; + depth = o.depth; + mask = o.mask; + silhouettePoints = o.silhouettePoints; + cam = o.cam; + renders = o.renders; + return *this; + } + + vpRBFeatureTrackerInput(const vpRBFeatureTrackerInput &other) + { + *this = other; + } + + vpRBFeatureTrackerInput &operator=(vpRBFeatureTrackerInput &&o) + { + I = std::move(o.I); + IRGB = std::move(o.IRGB); + depth = std::move(o.depth); + mask = std::move(o.mask); + silhouettePoints = std::move(o.silhouettePoints); + cam = std::move(o.cam); + renders = std::move(o.renders); + return *this; + } + + vpRBFeatureTrackerInput(vpRBFeatureTrackerInput &&other) + { + *this = std::move(other); + } +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBInitializationHelper.h b/modules/tracker/rbt/include/visp3/rbt/vpRBInitializationHelper.h new file mode 100644 index 0000000000..0f9a517a23 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBInitializationHelper.h @@ -0,0 +1,46 @@ +#ifndef VP_RB_INITIALIZATION_HELPER_H +#define VP_RB_INITIALIZATION_HELPER_H + +#include +#include +#include + +#include +#include + + +BEGIN_VISP_NAMESPACE +template +class vpImage; + +/** + * \brief A set of utilities to perform initialization. + * + * \ingroup group_rbt_core +*/ +class VISP_EXPORT vpRBInitializationHelper +{ +public: + void removeComment(std::ifstream &fileId); + void savePose(const std::string &filename) const; + + vpHomogeneousMatrix getPose() const { return m_cMo; } + + void setCameraParameters(const vpCameraParameters &cam) { m_cam = cam; } + +#ifdef VISP_HAVE_MODULE_GUI + template + void initClick(const vpImage &I, const std::string &initFile, bool displayHelp); + +#endif + +private: + std::string m_poseSavingFileName; + + vpHomogeneousMatrix m_cMo; + vpCameraParameters m_cam; +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h new file mode 100644 index 0000000000..6c0dffd5b5 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBKltTracker.h @@ -0,0 +1,266 @@ +/* + * 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. + */ + +/*! + \file vpRBKltTracker.h + \brief KLT features in the context of render based tracking +*/ +#ifndef VP_RB_KLT_TRACKER_H +#define VP_RB_KLT_TRACKER_H + +#include + +#if (defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)) +#define VP_HAVE_RB_KLT_TRACKER +#endif + +#if defined(VP_HAVE_RB_KLT_TRACKER) +#include +#include +#include +#include +#include + +#include + + +BEGIN_VISP_NAMESPACE + +/** + * \brief KLT-Based features + * + * \ingroup group_rbt_trackers +*/ +class VISP_EXPORT vpRBKltTracker : public vpRBFeatureTracker +{ +public: + vpRBKltTracker(); + + virtual ~vpRBKltTracker() = default; + + bool requiresRGB() const VP_OVERRIDE { return false; } + + bool requiresDepth() const VP_OVERRIDE { return false; } + + bool requiresSilhouetteCandidates() const VP_OVERRIDE { return false; } + + void onTrackingIterStart() VP_OVERRIDE { } + + void onTrackingIterEnd() VP_OVERRIDE { } + + void extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; + + void trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; + + void initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; + + void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) VP_OVERRIDE; + + void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth) const VP_OVERRIDE; + + /** + * \name Settings + * @{ + */ + + /** + * \brief Get the minimum acceptable number of points that should be tracked. If KLT tracking has less than this number of points + * The KLT tracking will be fully reinitialized. + */ + unsigned int getMinimumNumberOfPoints() const { return m_numPointsReinit; } + void setMinimumNumberOfPoints(unsigned int points) { m_numPointsReinit = points; } + + /** + * \brief Get the minimum distance that a candidate point should have to every other tracked point if it should be added. + * + * During tracking, KLT points are frequently sampled. This settings used to ensure that multiple klt points do not track the same 3D points + */ + double getMinimumDistanceNewPoints() const { return m_newPointsDistanceThreshold; } + void setMinimumDistanceNewPoints(double distance) { m_newPointsDistanceThreshold = distance; } + + /** + * \brief Return the number of pixels in the image border where points should not be tracked. + * Points that are near image borders are likely to be lost in the future. + */ + unsigned int getFilteringBorderSize() const { return m_border; } + void setFilteringBorderSize(unsigned int border) { m_border = border; } + + /** + * \brief Get the maximum reprojection error, in pixels, for a point to be considered as outlier. + * This reprojection error is computed between the tracked klt position in the image and the reprojection of the associated 3D point. + * If a point goes above this threshold, it is removed from tracking + * + * \return double + */ + double getFilteringMaxReprojectionError() const { return m_maxErrorOutliersPixels; } + void setFilteringMaxReprojectionError(double maxError) { m_maxErrorOutliersPixels = maxError; } + + /** + * \brief Returns whether the tracking algorithm should filter out points that are unlikely to be on the object according to the mask. + * If the mask is not computed beforehand, then it has no effect + */ + bool shouldUseMask() const { return m_useMask; } + void setShouldUseMask(bool useMask) { m_useMask = useMask; } + + /** + * \brief Returns the minimum mask confidence that a pixel should have if it should be kept during tracking. + * + * This value is between 0 and 1 + */ + float getMinimumMaskConfidence() const { return m_minMaskConfidence; } + void setMinimumMaskConfidence(float confidence) + { + if (confidence > 1.f || confidence < 0.f) { + throw vpException(vpException::badValue, "Mask confidence should be between 0 and 1"); + } + m_minMaskConfidence = confidence; + } + + /** + * \brief Get the underlying KLT tracker. Use this to read its settings. + */ + const vpKltOpencv &getKltTracker() const { return m_klt; } + /** + * \brief Get the underlying KLT tracker. Use this to modify its settings. + * + * \warning Only modify its tracking settings, not its state. + */ + vpKltOpencv &getKltTracker() { return m_klt; } + +#if defined(VISP_HAVE_NLOHMANN_JSON) + virtual void loadJsonConfiguration(const nlohmann::json &j) VP_OVERRIDE + { + vpRBFeatureTracker::loadJsonConfiguration(j); + + m_klt = j; + + setMinimumNumberOfPoints(j.value("minimumNumPoints", m_numPointsReinit)); + setMinimumDistanceNewPoints(j.value("newPointsMinPixelDistance", m_newPointsDistanceThreshold)); + setFilteringMaxReprojectionError(j.value("maxReprojectionErrorPixels", m_maxErrorOutliersPixels)); + setShouldUseMask(j.value("useMask", m_useMask)); + setMinimumMaskConfidence(j.value("minMaskConfidence", m_minMaskConfidence)); + } +#endif + + /** + * @} + * + */ + struct vpTrackedKltPoint + { + public: + vpHomogeneousMatrix cTo0; //! Initial pose of the object in the camera frame, acquired when the tracked point was first constructed + vpPoint oX; //! Tracked 3D point + vpColVector normal; //! Surface normal at this point, in the object frame + vpImagePoint currentPos; //! Current image coordinates, in normalized image coordinates + + inline double rotationDifferenceToInitial(const vpHomogeneousMatrix &oMc) + { + const vpHomogeneousMatrix cinitTc = cTo0 * oMc; + return cinitTc.getThetaUVector().getTheta(); + } + + inline double normalDotProd(const vpHomogeneousMatrix &cMo) + { + vpColVector cameraNormal = cMo.getRotationMatrix() * normal; + oX.changeFrame(cMo); + vpColVector dir({ -oX.get_X(), -oX.get_Y(), -oX.get_Z() }); + dir.normalize(); + return dir * cameraNormal; + } + + inline void update(const vpHomogeneousMatrix &cMo) + { + oX.changeFrame(cMo); + oX.projection(); + } + + inline void error(vpColVector &e, unsigned i) const + { + e[i * 2] = oX.get_x() - currentPos.get_u(); + e[i * 2 + 1] = oX.get_y() - currentPos.get_v(); + } + + inline double distance(const vpTrackedKltPoint &other) const + { + const double d = sqrt(std::pow(oX.get_oX() - other.oX.get_oX(), 2) + + std::pow(oX.get_oY() - other.oX.get_oY(), 2) + + std::pow(oX.get_oZ() - other.oX.get_oZ(), 2)); + return d; + } + + inline void interaction(vpMatrix &L, unsigned i) const + { + double x = oX.get_x(), y = oX.get_y(); + double xy = x * y; + double Zinv = 1.0 / oX.get_Z(); + L[i * 2][0] = -Zinv; + L[i * 2][1] = 0.0; + L[i * 2][2] = x * Zinv; + L[i * 2][3] = xy; + L[i * 2][4] = -(1.0 + x * x); + L[i * 2][5] = y; + + L[i * 2 + 1][0] = 0.0; + L[i * 2 + 1][1] = -Zinv; + L[i * 2 + 1][2] = y * Zinv; + L[i * 2 + 1][3] = 1.0 + y * y; + L[i * 2 + 1][4] = -xy; + L[i * 2 + 1][5] = -x; + } + }; + +private: + + void tryAddNewPoint(const vpRBFeatureTrackerInput &frame, std::map &points, + long id, const float u, const float v, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc); + + vpRobust m_robust; + + cv::Mat m_I, m_Iprev; + vpKltOpencv m_klt; + + unsigned int m_numPointsReinit; //! Minimum number of KLT points required to avoid performing reinitialization + double m_newPointsDistanceThreshold; //! Minimum distance (to the other tracked points) threshold for a new detected klt point to be considered as novel. In Pixels + unsigned int m_border; //! Image border size, where points should not be considered + + double m_maxErrorOutliersPixels; //! Max 3D reprojection error before a point is considered an outlier and rejected from tracking. In meters + + std::map m_points; + + bool m_useMask; + float m_minMaskConfidence; + +}; + +END_VISP_NAMESPACE + +#endif +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBProbabilistic3DDriftDetector.h b/modules/tracker/rbt/include/visp3/rbt/vpRBProbabilistic3DDriftDetector.h new file mode 100644 index 0000000000..a3cc421df3 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBProbabilistic3DDriftDetector.h @@ -0,0 +1,338 @@ +/* + * 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. + */ + +/*! + \file vpRBProbabilistic3DDriftDetector.h + \brief Drift detection through 3D point statistical description for the render-based tracker +*/ + +#ifndef VP_RB_PROBABILISTIC_3D_DRIFT_DETECTOR_H +#define VP_RB_PROBABILISTIC_3D_DRIFT_DETECTOR_H + +#include +#include +#include + +#include + +#include + +BEGIN_VISP_NAMESPACE + +template class vpImage; + +/** + * \ingroup group_rbt_drift + * + * \brief Algorithm that uses tracks object surface points in order to estimate the probability that tracking is successful. + * + * Given a set of surface points \f$ \mathbf{X}_0, ..., \mathbf{X}_N\f$, each point \f$\mathbf{X}_i\f$ being associated to: + * + * - a color distribution \f$\mathcal{N}(\mathbf{\bar c_i}, \mathbf{\Sigma_{c_i}^2})\f$, + * - its distance to the camera being \f$Z_i\f$, + * - its projection in the current color and depth images \f$\mathbf{I_c}, \mathbf{I_Z}\f$ having coordinates \f$u_i, v_i\f$. + * - its visibility \f$V(\mathbf{X_i})\f$, which is 1 if \f$u_i, v_i\f$ lie in the image, + * \f$Z_i\f$ is close to the rendered depth value + * and the normal at the surface marks the point as visible from the camera's point of view. + * + * + * We compute the probability that tracking is successful for a given pose \f$^{c}\mathbf{T}_o\f$ as: + * + * \f[ p(^{c}\mathbf{T}_o) = \frac{1}{\sum_{i=0}^N w_i \cdot V(\mathbf{X_i})}\sum_{i=0}^N w_i \cdot V(\mathbf{X_i}) \cdot p(\mathbf{X_i}) \f] + * + * with \f[ + * \begin{aligned} + * p(\mathbf{X_i}) &= p(\mathbf{I_c}(u_i, v_i)|\mathcal{N}(\mathbf{\bar c_i}, \mathbf{\Sigma_{c_i}^2})) \cdot p(\mathbf{I_Z}(u_i, v_i) | \mathcal{N}(Z_i, \sigma_Z^2)) \\ + * p(\mathbf{I_c}(u_i, v_i) | \mathcal{N}(\mathbf{\bar c_i}, \mathbf{\Sigma_{c_i}^2})) &= erfc(\frac{1}{\sqrt{2}}\lVert \frac{\mathbf{I_c}(u_i, v_i) - \mathbf{\bar c_i}}{diag(\mathbf{\Sigma_{c_i}})} \rVert_2) \\ + * p(\mathbf{I_Z}(u_i, v_i) | \mathcal{N}(Z_i, \sigma_Z^2)) &= erfc(\frac{1}{\sigma_Z \sqrt{2}}\mathbf{I_Z}(u_i, v_i) - Z_i) + * \end{aligned} + * \f] + * + * if the depth is unavailable, then we set \f$p(\mathbf{I_Z}(u_i, v_i) | \mathcal{N}(Z_i, \sigma_Z^2)) = 1\f$ + * + * Here, the color distribution is estimated online for each point separately using exponential moving average/variance techniques. + * For each point the update step is computed as \f$p(\mathbf{I_Z}(u_i, v_i) | \mathcal{N}(Z_i, \sigma_Z^2))\cdot \alpha\f$ where \f$\alpha\f$ is a fixed parameter. Larger values will lead to faster update rates and may be more beneficial for non lambertian materials. + * + * For the depth, \f$\sigma_Z\f$ is a fixed parameter to be tweaked by the user. + * + * Every time update() is called, the set of points \f$ \mathbf{X}_0, ..., \mathbf{X}_N, \f$ may grow larger. + * If a new candidate point is visible and is far enough from points already in the set, it is added to it. + */ +class VISP_EXPORT vpRBProbabilistic3DDriftDetector : public vpRBDriftDetector +{ +private: + + struct vpStored3DSurfaceColorPoint + { + + /** + * \brief Online estimation of a Gaussian color distribution \f$\mathcal{N}(\mathbf{\bar c}, \mathbf{\Sigma_c^2})\f$, Where \f$\mathbf{\Sigma_c^2}\f$ is a diagonal variance matrix \f$diag(\sigma_r^2, \sigma_g^2, \sigma_b^2)\f$. + * + * This class uses exponential moving average and variance estimation to approximage the distribution of the different RGB color components. + * + * It does not estimate the full covariance matrix, but rather the variance of the individual RGB components. + */ + struct ColorStatistics + { + ColorStatistics() = default; + + void init(const vpRGBf &c, const vpRGBf &var) + { + mean = c; + variance = var; + computeStddev(); + } + + /** + * \brief Update the color distribution with a new sample c. + * + * \param c + * \param weight The importance of c (between 0 and 1) in the distribution update (see Exponential moving average). A high value prioritizes the last seen values. + */ + void update(const vpRGBf &c, float weight) + { + const vpRGBf diff(c.R - mean.R, c.G - mean.G, c.B - mean.B); + vpRGBf diffSqr(std::pow(diff.R, 2), std::pow(diff.G, 2), std::pow(diff.B, 2)); + mean = mean + weight * diff; + variance = variance + weight * diffSqr; + computeStddev(); + } + + /** + * \brief Computes the probability that the input color was sampled from the estimated distribution. + * + * \param c + * \return the probability \f$ p(\mathbf{c} | \mathcal{N}(\mathbf{\bar c}, \mathbf{\Sigma_c})) \f$ + */ + double probability(const vpRGBf &c) + { + + const double dist = sqrt( + std::pow((c.R - mean.R) / (standardDev.R), 2) + + std::pow((c.G - mean.G) / (standardDev.G), 2) + + std::pow((c.B - mean.B) / (standardDev.B), 2)); + + const double proba = 1.0 - erf(dist / sqrt(2)); + + return proba; + } + + double trace() + { + return static_cast(variance.R + variance.G + variance.B); + } + + void computeStddev() + { + standardDev.R = sqrt(variance.R); + standardDev.G = sqrt(variance.G); + standardDev.B = sqrt(variance.B); + } + + vpRGBf mean; + vpRGBf variance; + vpRGBf standardDev; + }; + + inline void update(const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &cprevTo, const vpCameraParameters &cam) + { + fastProjection(cTo, cam, currX, projCurr, projCurrPx); + fastProjection(cprevTo, cam, prevX, projPrev, projPrevPx); + } + + inline double squaredDist(const std::array &p) const + { + return std::pow(p[0] - X[0], 2) + std::pow(p[1] - X[1], 2) + std::pow(p[2] - X[2], 2); + } + + inline void fastProjection(const vpHomogeneousMatrix &cTo, const vpCameraParameters &cam, + std::array &pC, std::array &proj, std::array &px) + { + const double *T = cTo.data; + pC[0] = (T[0] * X[0] + T[1] * X[1] + T[2] * X[2] + T[3]); + pC[1] = (T[4] * X[0] + T[5] * X[1] + T[6] * X[2] + T[7]); + pC[2] = (T[8] * X[0] + T[9] * X[1] + T[10] * X[2] + T[11]); + proj[0] = pC[0] / pC[2]; + proj[1] = pC[1] / pC[2]; + px[0] = static_cast((proj[0] * cam.get_px()) + cam.get_u0()); + px[1] = static_cast((proj[1] * cam.get_py()) + cam.get_v0()); + } + + void updateColor(const vpRGBf ¤tColor, float updateRate) + { + stats.update(currentColor, updateRate); + } + + vpRGBa getDisplayColor() const + { + return vpRGBa(static_cast(stats.mean.R), static_cast(stats.mean.G), static_cast(stats.mean.B)); + } + + std::array X; // Point position in object frame + ColorStatistics stats; //! Color statistics associated to this point + std::array currX, prevX; //! Point position in the current and previous camera frames + std::array projCurr, projPrev; // Projection in camera normalized coordinates of the point for the current and previous camera poses. + std::array projCurrPx, projPrevPx; // Projection in pixels of the point for the current and previous camera poses. + bool visible; // Whether the point is visible + }; + +public: + + vpRBProbabilistic3DDriftDetector() : m_colorUpdateRate(0.2), m_initialColorSigma(25.0), m_depthSigma(0.04), m_maxError3D(0.001), m_minDist3DNewPoint(0.003) + { } + + void update(const vpRBFeatureTrackerInput &previousFrame, const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &cprevTo) VP_OVERRIDE; + + /** + * \brief Returns the probability [0, 1] that tracking is successful. + * + */ + double getScore() const VP_OVERRIDE; + + bool hasDiverged() const VP_OVERRIDE; + + void display(const vpImage &I) VP_OVERRIDE; + + /** + * \name Settings + * @{ + */ + + /** + * \brief Get the minimum distance criterion (in meters) that is used + * when trying to add new points to track for the drift detection. + * + * A candidate surface point is compared to all the currently tracked surface point and + * if any of these points is below the minimum distance, the candidate is rejected. + * + * \return the minimum distance, in meters + */ + double getMinDistForNew3DPoints() const { return m_minDist3DNewPoint; } + + void setMinDistForNew3DPoints(double distance) + { + if (distance <= 0.0) { + throw vpException(vpException::badValue, "Distance criterion for candidate rejection should be greater than 0."); + } + m_minDist3DNewPoint = distance; + } + + /** + * \brief Returns the maximum 3D distance (in meters) above which a tracked surface point is rejected for the drift estimation step. + * + * The surface point's distance to the camera is compared to rendered depth. If the difference between the two is too great, it is rejected. + * + * This is mainly used to handle self occlusions. + */ + double getFilteringMax3DError() const { return m_maxError3D; } + + void setFilteringMax3DError(double maxError) + { + if (maxError <= 0.0) { + throw vpException(vpException::badValue, "Maximum 3D error for rejection should be greater than 0."); + } + m_maxError3D = maxError; + } + + /** + * \brief Get the standard deviation that is used when computing + * the probability that the observed depth Z is the correct depth given the rendered depth at the same pixel. + * + */ + double getDepthStandardDeviation() const { return m_depthSigma; } + void setDepthStandardDeviation(double sigma) + { + if (sigma < 0.0) { + throw vpException(vpException::badValue, "Depth standard deviation should be greater than 0"); + } + m_depthSigma = sigma; + } + + /** + * \brief Get the standard deviation that is used to initialize the color distribution when adding a new surface point. + * This standard deviation is applied on all color components. + * + */ + double getInitialColorStandardDeviation() const { return m_depthSigma; } + void setInitialColorStandardDeviation(double sigma) + { + if (sigma < 0.0) { + throw vpException(vpException::badValue, "Initial color standard deviation should be greater than 0"); + } + m_initialColorSigma = sigma; + } + + /** + * \brief Get the rate at which the colors of surface points are updated. + * + * Note that if depth is available, this component is further multiplied by the probability of depth being correct for a given point. + * + * A high value will lead to a fast update rate (short term memory), while a lower one will update slower. + * A slower update may lead to a more stable tracking score. A higher value may be better suited to non isotropic materials. + */ + double getColorUpdateRate() const { return m_colorUpdateRate; } + + /** + * \brief Set the update rate for the color distribution. It should be between 0 and 1. + * + * \param updateRate the update rate + */ + void setColorUpdateRate(double updateRate) + { + if (updateRate < 0.0 || updateRate > 1.f) { + throw vpException(vpException::badValue, "Color update rate should be between 0 and 1"); + } + m_colorUpdateRate = updateRate; + } + +#if defined(VISP_HAVE_NLOHMANN_JSON) + void loadJsonConfiguration(const nlohmann::json &) VP_OVERRIDE; +#endif + +/** + * @} + * End settings + */ + +private: + double m_colorUpdateRate; + double m_initialColorSigma; + double m_depthSigma; + double m_maxError3D; + double m_minDist3DNewPoint; + + double m_score; + + std::vector m_points; +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h new file mode 100644 index 0000000000..97997a5d46 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteCCDTracker.h @@ -0,0 +1,326 @@ +/* + * 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. + */ + +/*! + \file vpRBSilhouetteCCDTracker.h + \brief Color silhouette features +*/ +#ifndef VP_SILHOUETTE_CCD_TRACKER_H +#define VP_SILHOUETTE_CCD_TRACKER_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// #if defined(VISP_HAVE_SIMDLIB) +// #include +// #endif +#include +#include + +#include +#include +#include + +#if defined(VISP_HAVE_NLOHMANN_JSON) +#include VISP_NLOHMANN_JSON(json.hpp) +#endif + +BEGIN_VISP_NAMESPACE + + +enum vpRBSilhouetteCCDDisplayType +{ + SIMPLE = 0, + WEIGHT = 1, + ERROR = 2, + INVALID = 3 +}; + +class VISP_EXPORT vpCCDParameters +{ +public: + vpCCDParameters() : gamma_1(0.5), gamma_2(4), gamma_3(4), gamma_4(3), alpha(1.3), beta(0.06), kappa(0.5), covarianceIterDecreaseFactor(0.25), h(40), delta_h(1), phi_dim(6) + { } + + + ~vpCCDParameters() = default; + /** + * \brief Curve uncertainty computation hyperparameter + * Recommended to leave fixed + */ + double gamma_1; + /** + * \brief Curve uncertainty computation hyperparameter + * Recommended to leave fixed + */ + double gamma_2; + /** + * \brief Curve uncertainty computation hyperparameter + * Recommended to leave fixed + */ + double gamma_3; + /** + * \brief Curve uncertainty computation hyperparameter + * Recommended to leave fixed + */ + double gamma_4; + double alpha; + double beta; + + /** + * \brief Bias to the diagonal of the covariance of the color statistics of a single pixel. + * Used to avoid singularities and degenerate cases + * + * The final pixel color covariance will be kappa * I(3) + covariance. + * + */ + double kappa; + /** + * + * \brief From the CCD paper: + * maximum decrease of the covariance within one iteration step. Between 0 and 1 + * If c2 is too high, the covariance declines slowly. Hence, a small number of iterations is + * necessary. If c2 is too small, the CCD algorithm may converge to a wrong solution. + * it is recommended to leave this value fixed + */ + double covarianceIterDecreaseFactor; + /** + * \brief Size of the vicinity that is used to compute statistics and error. + * Length of the line along the normal (and the opposite direction). To subsample the line, set delta_h > 1. + * Number of pixels used is computed as 2 * floor(h/delta_h). If you expect large motions, set a large value. + * If you want to reduce computation time, decrease this value or increase delta_h + * Recommended value: 4 or above (this is dependent on image resolution) + */ + int h; + /** + * \brief Sample step when computing statistics and errors. + * Increase this value to decrease computation time, at the risk of obtaining inacurrate statistics. + */ + int delta_h; + /** + * \brief Number of parameters estimated by CCD. Either 6 or 8. + * Leave this fixed + */ + int phi_dim; +}; + +#if defined(VISP_HAVE_NLOHMANN_JSON) +inline void from_json(const nlohmann::json &j, vpCCDParameters &ccdParameters) +{ + ccdParameters.alpha = j.value("alpha", ccdParameters.alpha); + ccdParameters.beta = j.value("beta", ccdParameters.beta); + ccdParameters.kappa = j.value("kappa", ccdParameters.kappa); + ccdParameters.covarianceIterDecreaseFactor = j.value("covarianceIterDecreaseFactor", + ccdParameters.covarianceIterDecreaseFactor); + ccdParameters.h = j.value("h", ccdParameters.h); + ccdParameters.delta_h = j.value("delta_h", ccdParameters.delta_h); + ccdParameters.phi_dim = j.value("phi_dim", ccdParameters.phi_dim); + if (j.contains("gamma")) { + nlohmann::json gammaj = j["gamma"]; + if (!gammaj.is_array() || gammaj.size() != 4) { + throw vpException(vpException::ioError, "CCD parameters: tried to read gamma values from something that is not a 4-sized float array"); + } + ccdParameters.gamma_1 = gammaj[0]; + ccdParameters.gamma_2 = gammaj[1]; + ccdParameters.gamma_3 = gammaj[2]; + ccdParameters.gamma_4 = gammaj[3]; + } +} +#endif + +class VISP_EXPORT vpCCDStatistics +{ +public: + vpMatrix vic; //! Vicinity data + vpMatrix mean_vic; //! Mean + vpMatrix cov_vic; //! Covariance + vpMatrix nv; //! Normal vector + vpMatrix imgPoints; //! Img pixels + vpMatrix weight; //! Whether this pixel is the object + + void reinit(int resolution, unsigned normalPointsNumber) + { + nv.resize(resolution, 2, false, false); + mean_vic.resize(resolution, 6, false, false); + cov_vic.resize(resolution, 18, false, false); + vic.resize(resolution, 20 * normalPointsNumber, false, false); + imgPoints.resize(resolution, 2 * 3 * normalPointsNumber, false, false); + weight.resize(resolution, 2 * normalPointsNumber, false, false); + } +}; + +/** + * \brief Tracking based on the Contracting Curve Density algorithm. + * + * \ingroup group_rbt_trackers + */ +class VISP_EXPORT vpRBSilhouetteCCDTracker : public vpRBFeatureTracker +{ +public: + + vpRBSilhouetteCCDTracker(); + virtual ~vpRBSilhouetteCCDTracker() = default; + + bool requiresRGB() const VP_OVERRIDE { return true; } + bool requiresDepth() const VP_OVERRIDE { return false; } + bool requiresSilhouetteCandidates() const VP_OVERRIDE { return true; } + + /** + * \name Settings + * @{ + */ + void setCCDParameters(const vpCCDParameters ¶meters) { m_ccdParameters = parameters; } + vpCCDParameters getCCDParameters() const { return m_ccdParameters; } + //void computeMask(const vpImage &render, vpCCDStatistics &stats); + + /** + * \brief Returns the amount of temporal smoothing applied when computing the tracking error and its jacobian. + * This factor is used to interpolate with the error computed on the previous frame for the features selected at the current iteration + * Temporal smoothing may help smooth out the motion and reduce jitter. + */ + double getTemporalSmoothingFactor() const { return m_temporalSmoothingFac; } + /** + * \brief Sets the temporal smoothing factor. + * + * \see getTemporalSmoothingFactor + * + * @param factor the new temporal smoothing factor. Should be greater than 0 + */ + void setTemporalSmoothingFactor(double factor) + { + if (factor < 0.0) { + throw vpException(vpException::badValue, "Temporal smoothing factor should be equal to or greater than 0"); + } + m_temporalSmoothingFac = factor; + + } + + /** + * \brief Returns whether the tracking algorithm should filter out points that are unlikely to be on the object according to the mask. + * If the mask is not computed beforehand, then it has no effect + */ + bool shouldUseMask() const { return m_useMask; } + void setShouldUseMask(bool useMask) { m_useMask = useMask; } + + /** + * \brief Returns the minimum mask gradient required for a silhouette point to be considered + * + * This value is between 0 and 1 + */ + float getMinimumMaskConfidence() const { return m_minMaskConfidence; } + void setMinimumMaskConfidence(float confidence) + { + + m_minMaskConfidence = confidence; + } + + void setDisplayType(vpRBSilhouetteCCDDisplayType type) + { + m_displayType = type; + } + + /** + * @} + */ + + void onTrackingIterStart() VP_OVERRIDE { } + void onTrackingIterEnd() VP_OVERRIDE { } + + double getVVSTrackerWeight() const VP_OVERRIDE { return m_userVvsWeight / (10 * m_error.size()); } + + void extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; + void trackFeatures(const vpRBFeatureTrackerInput & /*frame*/, const vpRBFeatureTrackerInput & /*previousFrame*/, const vpHomogeneousMatrix & /*cMo*/) VP_OVERRIDE { } + + void initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; + void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) VP_OVERRIDE; + void updateCovariance(const double /*lambda*/) VP_OVERRIDE + { + m_cov = m_sigma; + } + + void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth) const VP_OVERRIDE; + +#if defined(VISP_HAVE_NLOHMANN_JSON) + virtual void loadJsonConfiguration(const nlohmann::json &j) VP_OVERRIDE + { + vpRBFeatureTracker::loadJsonConfiguration(j); + + m_vvsConvergenceThreshold = j.value("convergenceThreshold", m_vvsConvergenceThreshold); + setTemporalSmoothingFactor(j.value("temporalSmoothing", m_temporalSmoothingFac)); + setShouldUseMask(j.value("useMask", m_useMask)); + setMinimumMaskConfidence(j.value("minMaskConfidence", m_minMaskConfidence)); + + m_ccdParameters = j.value("ccd", m_ccdParameters); + } + +#endif + +protected: + void updateCCDPoints(const vpHomogeneousMatrix &cMo); + void computeLocalStatistics(const vpImage &I, vpCCDStatistics &stats); + void computeErrorAndInteractionMatrix(); + double computeMaskGradient(const vpImage &mask, const vpRBSilhouetteControlPoint &pccd) const; + + + vpCCDParameters m_ccdParameters; + + std::vector m_controlPoints; //! Silhouette points where to compute CCD statistics + vpRobust m_robust; + + vpCCDStatistics m_stats; + vpCCDStatistics m_prevStats; + + vpMatrix m_sigma; + + double m_vvsConvergenceThreshold; + double tol; + + std::vector m_gradients; + std::vector m_hessians; + vpColVector m_gradient; //! Sum of local gradients + vpMatrix m_hessian; //! Sum of local hessians + double m_temporalSmoothingFac; //! Smoothing factor used to integrate data from the previous frame. + + bool m_useMask; + double m_minMaskConfidence; + + vpRBSilhouetteCCDDisplayType m_displayType; +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h new file mode 100644 index 0000000000..9e1a18f173 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteControlPoint.h @@ -0,0 +1,164 @@ +/* + * 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. + */ + +/*! + \file vpRBSilhouetteControlPoint.h + \brief Trackable silhouette point representation +*/ + +#ifndef VP_RB_SILHOUETTE_CONTROL_POINT_H +#define VP_RB_SILHOUETTE_CONTROL_POINT_H + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +BEGIN_VISP_NAMESPACE + +/*! + \brief Trackable silhouette point representation + + \ingroup group_rbt_core +*/ +class VISP_EXPORT vpRBSilhouetteControlPoint +{ +private: + + double rho, theta; + double thetaInit; + double delta; + int sign; + //double a,b,c; + vpFeatureLine featureline; + vpLine line; + + std::vector m_candidates; + unsigned int m_numCandidates; + const vpMe *m_me; + vpMeSite s; + + //! Normal to surface where the control point lies + vpColVector norm; + vpColVector normw; + + bool m_valid; + + bool m_isSilhouette; + + const vpCameraParameters *m_cam; +public: + + vpImagePoint icpoint; + + // the 3D point + vpPoint cpoint; + vpPoint cpointo; + + double xs, ys, nxs, nys, Zs; + +public: + + void init(); + vpRBSilhouetteControlPoint(); + vpRBSilhouetteControlPoint(const vpRBSilhouetteControlPoint &meTracker); + vpRBSilhouetteControlPoint(const vpRBSilhouetteControlPoint &&meTracker); + vpRBSilhouetteControlPoint &operator=(const vpRBSilhouetteControlPoint &meTracker); + vpRBSilhouetteControlPoint &operator=(const vpRBSilhouetteControlPoint &&meTracker); + ~vpRBSilhouetteControlPoint() = default; + + /** + * @brief Set the number of candidates to use for multiple hypotheses testing + * + * @param numCandidates + */ + void setNumCandidates(unsigned numCandidates) { m_numCandidates = numCandidates; } + unsigned getNumCandidates() const { return m_numCandidates; } + void setValid(bool valid) { m_valid = valid; } + bool isValid() const { return m_valid; } + + + + const vpCameraParameters &getCameraParameters() const { return *m_cam; } + bool siteIsValid() const { return s.getState() == vpMeSite::NO_SUPPRESSION; } + const vpMeSite &getSite() const { return s; } + vpMeSite &getSite() { return s; } + const vpFeatureLine &getFeatureLine() const { return featureline; } + const vpLine &getLine() const { return line; } + double getTheta() const { return theta; } + bool isSilhouette() const { return m_isSilhouette; } + + void initControlPoint(const vpImage &I, double cvlt); + void detectSilhouette(const vpImage &I); + void buildPoint(int n, int m, const double &Z, double orient, const vpColVector &normo, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc, const vpCameraParameters &cam, const vpMe &me); + void buildSilhouettePoint(int n, int m, const double &Z, double orient, const vpColVector &normo, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc, const vpCameraParameters &cam); + + void update(const vpHomogeneousMatrix &_cMo); + void updateSilhouettePoint(const vpHomogeneousMatrix &_cMo); + + /** + * @brief Track the moving edge at this point retaining only the hypothesis with the highest likelihood + * + * @param I The image in which to track + */ + void track(const vpImage &I); + + /** + * @brief Track the moving edge and retain the best numCandidates hypotheses + * + * @param I The image in which to track + * + * \see setNumCandidates + */ + void trackMultipleHypotheses(const vpImage &I); + + void computeMeInteractionMatrixError(const vpHomogeneousMatrix &cMo, unsigned int i, vpMatrix &L, vpColVector &e); + void computeMeInteractionMatrixErrorMH(const vpHomogeneousMatrix &cMo, unsigned int i, vpMatrix &L, vpColVector &e); + +private: + bool isLineDegenerate() const; + + int outOfImage(int i, int j, int half, int rows, int cols) const; + int outOfImage(const vpImagePoint &iP, int half, int rows, int cols) const; + void buildPlane(const vpPoint &pointn, const vpColVector &normal, vpPlane &plane); + void buildPLine(const vpHomogeneousMatrix &oMc); + +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h new file mode 100644 index 0000000000..fc25cc2a14 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouetteMeTracker.h @@ -0,0 +1,186 @@ +/* + * 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. + */ + +/*! + \file vpRBSilhouetteMeTracker.h + \brief Moving edge tracking for depth-extracted object contours +*/ +#ifndef VP_RB_SILHOUETTE_ME_TRACKER_H +#define VP_RB_SILHOUETTE_ME_TRACKER_H + +#include +#include +#include + +BEGIN_VISP_NAMESPACE +/** + * \brief Moving edge feature tracking from depth-extracted object contours + * + * \ingroup group_rbt_trackers +*/ +class VISP_EXPORT vpRBSilhouetteMeTracker : public vpRBFeatureTracker +{ +public: + + vpRBSilhouetteMeTracker() : + vpRBFeatureTracker(), m_me(), m_numCandidates(1), m_robustMadMin(1.0), m_globalVVSConvergenceThreshold(1.0), + m_singlePointConvergedThresholdPixels(3), m_useMask(false), m_minMaskConfidence(0.f) + { } + + virtual ~vpRBSilhouetteMeTracker() = default; + + bool requiresRGB() const VP_OVERRIDE { return false; } + + bool requiresDepth() const VP_OVERRIDE { return false; } + + bool requiresSilhouetteCandidates() const VP_OVERRIDE { return true; } + + void setMovingEdge(const vpMe &me) { m_me = me; } + + void onTrackingIterStart() VP_OVERRIDE + { + m_controlPoints.clear(); + } + + void onTrackingIterEnd() VP_OVERRIDE { } + + /** + * @brief Extract the geometric features from the list of collected silhouette points + */ + void extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; + + void trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; + + void initVVS(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &cMo) VP_OVERRIDE; + + void computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) VP_OVERRIDE; + + void display(const vpCameraParameters &cam, const vpImage &I, const vpImage &IRGB, const vpImage &depth) const VP_OVERRIDE; + + /** + * \name Settings + * @{ + */ + const vpMe &getMe() const { return m_me; } + vpMe &getMe() { return m_me; } + + unsigned int getNumCandidates() const { return m_numCandidates; } + void setNumCandidates(unsigned int candidates) + { + if (candidates == 0) { + throw vpException(vpException::badValue, "Cannot set a number of candidates equal to zero"); + } + m_numCandidates = candidates; + } + + double getMinRobustThreshold() const { return m_robustMadMin; } + void setMinRobustThreshold(double threshold) + { + if (threshold < 0) { + throw vpException(vpException::badValue, "Robust M estimator min threshold should be greater or equal to 0."); + } + m_robustMadMin = threshold; + } + + /** + * \brief Returns whether the tracking algorithm should filter out points that are unlikely to be on the object according to the mask. + * If the mask is not computed beforehand, then it has no effect + */ + bool shouldUseMask() const { return m_useMask; } + void setShouldUseMask(bool useMask) { m_useMask = useMask; } + + /** + * \brief Returns the minimum mask confidence that a pixel linked to depth point should have if it should be kept during tracking. + * + * This value is between 0 and 1 + */ + float getMinimumMaskConfidence() const { return m_minMaskConfidence; } + void setMinimumMaskConfidence(float confidence) + { + if (confidence > 1.f || confidence < 0.f) { + throw vpException(vpException::badValue, "Mask confidence should be between 0 and 1"); + } + m_minMaskConfidence = confidence; + } + + double getSinglePointConvergenceThreshold() const { return m_singlePointConvergedThresholdPixels; } + void setSinglePointConvergenceThreshold(double threshold) + { + if (threshold < 0.0) { + throw vpException(vpException::badValue, "Convergence threshold should be null or positive"); + } + m_singlePointConvergedThresholdPixels = threshold; + } + + double getGlobalConvergenceMinimumRatio() const { return m_globalVVSConvergenceThreshold; } + void setGlobalConvergenceMinimumRatio(double threshold) + { + if (threshold < 0.0 || threshold > 1.0) { + throw vpException(vpException::badValue, "Minimum converged ratio be between 0 and 1"); + } + m_globalVVSConvergenceThreshold = threshold; + } + +#if defined(VISP_HAVE_NLOHMANN_JSON) + virtual void loadJsonConfiguration(const nlohmann::json &j) VP_OVERRIDE + { + vpRBFeatureTracker::loadJsonConfiguration(j); + setNumCandidates(j.value("numCandidates", m_numCandidates)); + setSinglePointConvergenceThreshold(j.value("convergencePixelThreshold", m_singlePointConvergedThresholdPixels)); + setGlobalConvergenceMinimumRatio(j.value("convergenceRatio", m_globalVVSConvergenceThreshold)); + m_me = j.value("movingEdge", m_me); + setShouldUseMask(j.value("useMask", m_useMask)); + setMinimumMaskConfidence(j.value("minMaskConfidence", m_minMaskConfidence)); + // m_me.setThresholdMarginRatio(-1.0); + // m_me.setMinThreshold(-1.0); + } +#endif + + /** + * \name Settings + * @} + */ + +private: + + std::vector m_controlPoints; + vpMe m_me; //! Moving edge settings + unsigned int m_numCandidates; //! Number of best candidates kept when finding correspondence points + vpRobust m_robust; //! M-Estimator to filter outliers + double m_robustMadMin; + double m_globalVVSConvergenceThreshold; //! Percentage of control points that should have converged to consider VVS as successful + double m_singlePointConvergedThresholdPixels; //! Whether a single Control point is considered as converged + bool m_useMask; + float m_minMaskConfidence; +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePoint.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePoint.h new file mode 100644 index 0000000000..e0a697261f --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePoint.h @@ -0,0 +1,62 @@ +/* + * 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. + */ + +/*! + \file vpRBSilhouettePoint.h + \brief Silhouette point simple candidate representation +*/ +#ifndef VP_RB_SILHOUETTE_POINT_H +#define VP_RB_SILHOUETTE_POINT_H + +#include +#include + +BEGIN_VISP_NAMESPACE +/*! + \brief Silhouette point simple candidate representation. + \ingroup group_rbt_core +*/ +class VISP_EXPORT vpRBSilhouettePoint +{ +public: + unsigned int i, j; //! Pixel coordinates of the silhouette point + vpColVector normal; //! Normal to the silhouette at point i,j, in world frame + double orientation; //! angle of the normal in the image. + double Z; //! Point depth + + vpRBSilhouettePoint(unsigned int i, unsigned int j, const vpColVector &normal, double orientation, double Z) : + i(i), j(j), normal(normal), orientation(orientation), Z(Z) + { } + +}; + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePointsExtractionSettings.h b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePointsExtractionSettings.h new file mode 100644 index 0000000000..50a8a2976d --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBSilhouettePointsExtractionSettings.h @@ -0,0 +1,143 @@ +/* + * 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. + */ + +/*! + \file vpRBSilhouettePointsExtractionSettings.h + \brief Silhouette point extraction settings +*/ + +#ifndef VP_RB_SILHOUETTE_POINTS_EXTRACTION_SETTINGS_H +#define VP_RB_SILHOUETTE_POINTS_EXTRACTION_SETTINGS_H + +#include + +#if defined(VISP_HAVE_PANDA3D) +#include +#include +#include +#include + + +#if defined(VISP_HAVE_NLOHMANN_JSON) +#include VISP_NLOHMANN_JSON(json.hpp) +#endif + + +BEGIN_VISP_NAMESPACE + +class vpRBSilhouettePoint; +/*! + \brief Silhouette point extraction settings + \ingroup group_rbt_core +*/ +class VISP_EXPORT vpSilhouettePointsExtractionSettings +{ + +private: + unsigned int m_sampleStep; //! Step with which to sample the image to acquire candidates + int m_maxNumPoints; //! Max number of points to keep + unsigned int m_border; //! Border rejection parameter: do not seek candidates that are too close to image border + + double m_depthThreshold; + bool m_thresholdIsRelative; + bool m_preferPreviousPoints; + + void sampleWithoutReplacement(unsigned int count, unsigned int vectorSize, std::vector &indices, vpUniRand &random) const + { + count = std::min(count, vectorSize); + indices.resize(count); + unsigned int added = 0; + for (unsigned i = 0; i < vectorSize; ++i) { + double randomVal = random.uniform(0.0, 1.0); + if ((vectorSize - i) * randomVal < (count - added)) { + indices[added++] = i; + } + if (added == count) { + break; + } + } + } + +public: + + vpSilhouettePointsExtractionSettings(); + vpSilhouettePointsExtractionSettings(const vpSilhouettePointsExtractionSettings &rend); + ~vpSilhouettePointsExtractionSettings() = default; + const vpSilhouettePointsExtractionSettings &operator=(const vpSilhouettePointsExtractionSettings &rend); + + double getThreshold() const { return m_depthThreshold; } + void setThreshold(double lambda) { m_depthThreshold = lambda; } + bool thresholdIsRelative() const { return m_thresholdIsRelative; } + void setThresholdIsRelative(bool isRelative) { m_thresholdIsRelative = isRelative; } + bool preferPreviousPoints() const { return m_preferPreviousPoints; } + void setPreferPreviousPoints(bool prefer) { m_preferPreviousPoints = prefer; } + + + int getMaxCandidates() const { return m_maxNumPoints; } + void setMaxCandidates(int maxCandidates) { m_maxNumPoints = maxCandidates; } + unsigned int getSampleStep() const { return m_sampleStep; } + void setSampleStep(unsigned int a) + { + if (m_sampleStep == 0) { + throw vpException(vpException::badValue, "Sample step should be greater than 0"); + } + m_sampleStep = a; + } + + std::vector> getSilhouetteCandidates( + const vpImage &validSilhouette, const vpImage &renderDepth, + const vpCameraParameters &cam, const vpHomogeneousMatrix &cTcp, + const std::vector &previousPoints, long randomSeed = 41) const; + +#if defined(VISP_HAVE_NLOHMANN_JSON) + inline friend void from_json(const nlohmann::json &j, vpSilhouettePointsExtractionSettings &settings); +#endif + +}; + +#if defined(VISP_HAVE_NLOHMANN_JSON) +inline void from_json(const nlohmann::json &j, vpSilhouettePointsExtractionSettings &settings) +{ + nlohmann::json thresholdSettings = j.at("threshold"); + std::string thresholdType = thresholdSettings.at("type"); + settings.m_thresholdIsRelative = thresholdType == "relative"; + settings.m_depthThreshold = thresholdSettings.at("value"); + + nlohmann::json samplingSettings = j.at("sampling"); + settings.m_preferPreviousPoints = samplingSettings.at("reusePreviousPoints"); + settings.m_maxNumPoints = samplingSettings.at("numPoints"); + settings.setSampleStep(samplingSettings.at("samplingRate")); +} +#endif + +END_VISP_NAMESPACE + +#endif +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h b/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h new file mode 100644 index 0000000000..838c6ca1fc --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h @@ -0,0 +1,281 @@ +/* + * 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. + */ + +/*! + \file vpRBTracker.h + \brief Render-Based Tracker +*/ +#ifndef VP_RB_TRACKER_H +#define VP_RB_TRACKER_H + +#include + +#if defined(VISP_HAVE_PANDA3D) + +#include +#include +#include +#include +#include + +#include + +#include + +#if defined(VISP_HAVE_NLOHMANN_JSON) +#include VISP_NLOHMANN_JSON(json_fwd.hpp) +#endif + +BEGIN_VISP_NAMESPACE + +class vpObjectMask; +class vpRBDriftDetector; +class vpRBVisualOdometry; + +/** + * \brief + * + * \ingroup group_rbt_core +*/ +class VISP_EXPORT vpRBTracker +{ +public: + + vpRBTracker(); + + ~vpRBTracker() = default; + + /** + * \name Information retrieval + * @{ + */ + void getPose(vpHomogeneousMatrix &cMo) const; + void setPose(const vpHomogeneousMatrix &cMo); + vpObjectCentricRenderer &getRenderer(); + const vpRBFeatureTrackerInput &getMostRecentFrame() const { return m_currentFrame; } + const vpRBTrackerLogger &getLogger() const { return m_logger; } + + vpMatrix getCovariance() const; + + /** + * @} + */ + + /** + * \name Settings + * @{ + */ + void addTracker(std::shared_ptr tracker); + void setupRenderer(const std::string &file); + std::string getModelPath() const { return m_modelPath; } + void setModelPath(const std::string &path); + + std::vector> getFeatureTrackers() { return m_trackers; } + + vpCameraParameters getCameraParameters() const; + void setCameraParameters(const vpCameraParameters &cam, unsigned h, unsigned w); + + unsigned int getImageWidth() const { return m_imageWidth; } + unsigned int getImageHeight() const { return m_imageHeight; } + + vpSilhouettePointsExtractionSettings getSilhouetteExtractionParameters() const + { + return m_depthSilhouetteSettings; + } + + void setSilhouetteExtractionParameters(const vpSilhouettePointsExtractionSettings &settings); + + double getOptimizationGain() const { return m_lambda; } + void setOptimizationGain(double lambda) + { + if (lambda < 0.0) { + throw vpException(vpException::badValue, "Optimization gain should be greater to zero"); + } + m_lambda = lambda; + } + unsigned int getMaxOptimizationIters() const { return m_vvsIterations; } + void setMaxOptimizationIters(unsigned int iters) + { + if (iters == 0) { + throw vpException(vpException::badValue, "Max number of iterations must be greater than zero"); + } + m_vvsIterations = iters; + } + + double getOptimizationInitialMu() const { return m_muInit; } + void setOptimizationInitialMu(double mu) + { + if (mu < 0.0) { + throw vpException(vpException::badValue, "Optimization gain should be greater or equal to zero"); + } + m_muInit = mu; + } + + double getOptimizationMuIterFactor() const { return m_muIterFactor; } + void setOptimizationMuIterFactor(double factor) + { + if (factor < 0.0) { + throw vpException(vpException::badValue, "Optimization gain should be greater or equal to zero"); + } + m_muIterFactor = factor; + } + + std::shared_ptr getDriftDetector() const { return m_driftDetector; } + void setDriftDetector(const std::shared_ptr &detector) + { + m_driftDetector = detector; + } + + std::shared_ptr getObjectSegmentationMethod() const { return m_mask; } + void setObjectSegmentationMethod(const std::shared_ptr &mask) + { + m_mask = mask; + } + + std::shared_ptr getOdometryMethod() const { return m_odometry; } + void setOdometryMethod(const std::shared_ptr &odometry) + { + m_odometry = odometry; + } + + /*! + * Get verbosity mode. + * \return true when verbosity is enabled, false otherwise. + */ + bool getVerbose() + { + return m_verbose; + } + + /*! + * Enable/disable verbosity mode. + * \param verbose : When true verbose mode is enabled. When false verbosity is disabled. + */ + void setVerbose(bool verbose) + { + m_verbose = verbose; + } + +#if defined(VISP_HAVE_NLOHMANN_JSON) + void loadConfigurationFile(const std::string &filename); + void loadConfiguration(const nlohmann::json &j); +#endif + + /** + * @} + */ + + void reset(); + + /** + * \name Tracking + * @{ + */ + void startTracking(); + void track(const vpImage &I); + void track(const vpImage &I, const vpImage &IRGB); + void track(const vpImage &I, const vpImage &IRGB, const vpImage &depth); + /** + * @} + */ + + /** + * \name Display + * @{ + */ + void displayMask(vpImage &Imask) const; + void display(const vpImage &I, const vpImage &IRGB, const vpImage &depth); + /** + * @} + */ + +#ifdef VISP_HAVE_MODULE_GUI + void initClick(const vpImage &I, const std::string &initFile, bool displayHelp); +#endif + +protected: + + void track(vpRBFeatureTrackerInput &input); + void updateRender(vpRBFeatureTrackerInput &frame); + + std::vector extractSilhouettePoints( + const vpImage &Inorm, const vpImage &Idepth, + const vpImage &Ior, const vpImage &Ivalid, + const vpCameraParameters &cam, const vpHomogeneousMatrix &cTcp); + + template + void checkDimensionsOrThrow(const vpImage &I, const std::string &imgType) const + { + if (I.getRows() != m_imageHeight || I.getCols() != m_imageWidth) { + std::stringstream ss; + ss << "vpRBTracker: dimension error: expected " << imgType; + ss << " image to have the following resolution " << m_imageWidth << " x " << m_imageHeight; + ss << ", but got " << I.getCols() << " x " << I.getRows(); + throw vpException(vpException::dimensionError, ss.str()); + } + } + + bool m_firstIteration; //! Whether this is the first iteration + + std::vector> m_trackers; //! List of trackers + + vpRBFeatureTrackerInput m_currentFrame; + vpRBFeatureTrackerInput m_previousFrame; + + std::string m_modelPath; + vpHomogeneousMatrix m_cMo; + vpHomogeneousMatrix m_cMoPrev; + vpCameraParameters m_cam; + + double m_lambda; //! VVS gain + unsigned m_vvsIterations; //! Max number of VVS iterations + double m_muInit; //! Initial mu value for Levenberg-Marquardt + double m_muIterFactor; //! Factor with which to multiply mu at every iteration during VVS. + + vpSilhouettePointsExtractionSettings m_depthSilhouetteSettings; + vpPanda3DRenderParameters m_rendererSettings; + vpObjectCentricRenderer m_renderer; + + unsigned m_imageHeight, m_imageWidth; //! Color and render image dimensions + + vpRBTrackerLogger m_logger; + bool m_verbose; + + std::shared_ptr m_mask; + std::shared_ptr m_driftDetector; + std::shared_ptr m_odometry; + +}; + +END_VISP_NAMESPACE + + +#endif +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h b/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h new file mode 100644 index 0000000000..44f5c60e12 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h @@ -0,0 +1,189 @@ +/* + * 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. + */ + +/*! + \file vpRBTrackerLogger.h + \brief Information storage for render based tracking process. +*/ +#ifndef VP_RB_TRACKER_LOGGER_H +#define VP_RB_TRACKER_LOGGER_H + +#include +#include +#include + +#if defined(VISP_HAVE_NLOHMANN_JSON) +#include VISP_NLOHMANN_JSON(json.hpp) +#endif + +BEGIN_VISP_NAMESPACE +/*! + \brief Information storage for render based tracking process. + + \ingroup group_rbt_core +*/ + +class vpRBTrackerLogger; + +std::ostream &operator<<(std::ostream &s, const vpRBTrackerLogger &I); + +class VISP_EXPORT vpRBTrackerLogger +{ +public: + void reset() + { + m_renderTime = 0.0; + m_silhouetteExtractionTime = 0.0; + m_odometryTime = 0.0; + m_driftTime = 0.0; + + m_trackerIterStartTime.clear(); + m_trackerFeatureExtractionTime.clear(); + + m_trackerFeatureTrackingTime.clear(); + m_trackerVVSIterTimes.clear(); + } + + friend std::ostream &operator<<(std::ostream &, const vpRBTrackerLogger &); + + void startTimer() { m_startTime = vpTime::measureTimeMs(); } + double endTimer() + { + if (m_startTime < 0.f) throw vpException(vpException::notInitialized, "Tried to query timer without starting it."); + double elapsed = vpTime::measureTimeMs() - m_startTime; + m_startTime = -1.f; + return elapsed; + } + + void setRenderTime(double elapsed) { m_renderTime = elapsed; } + void setSilhouetteTime(double elapsed) { m_silhouetteExtractionTime = elapsed; } + void setMaskTime(double elapsed) { m_maskTime = elapsed; } + + void insertTrackerTime(std::map> &map, int id, double elapsed) + { + if (map.find(id) == map.end()) { + map.insert(std::make_pair(id, std::vector())); + } + map.find(id)->second.push_back(elapsed); + } + void addTrackerVVSTime(int id, double elapsed) + { + insertTrackerTime(m_trackerVVSIterTimes, id, elapsed); + } + + void setTrackerIterStartTime(int id, double elapsed) + { + m_trackerIterStartTime[id] = elapsed; + } + + void setTrackerFeatureExtractionTime(int id, double elapsed) + { + m_trackerFeatureExtractionTime[id] = elapsed; + } + + void setTrackerFeatureTrackingTime(int id, double elapsed) + { + m_trackerFeatureTrackingTime[id] = elapsed; + } + + void setInitVVSTime(int id, double elapsed) + { + m_trackerInitVVSTime[id] = elapsed; + } + + void setDriftDetectionTime(double elapsed) + { + m_driftTime = elapsed; + } + + void setOdometryTime(double elapsed) + { + m_odometryTime = elapsed; + } + +private: + double m_startTime; + double m_renderTime; + double m_silhouetteExtractionTime; + double m_maskTime; + double m_driftTime; + double m_odometryTime; + std::map> m_trackerVVSIterTimes; + + std::map m_trackerIterStartTime; + + std::map m_trackerFeatureExtractionTime; + + std::map m_trackerFeatureTrackingTime; + std::map m_trackerInitVVSTime; + std::map m_trackerNumFeatures; + +}; + +std::ostream &operator<<(std::ostream &out, const vpRBTrackerLogger &timer) +{ + const auto default_precision { out.precision() }; + auto flags = out.flags(); + out << std::setprecision(2) << std::fixed; + out << "====================================================" << std::endl; + out << "Render: " << timer.m_renderTime << "ms" << std::endl; + out << "Mask: " << timer.m_maskTime << "ms" << std::endl; + out << "Drift: " << timer.m_driftTime << "ms" << std::endl; + out << "Odometry: " << timer.m_odometryTime << "ms" << std::endl; + out << "Silhouette extraction: " << timer.m_silhouetteExtractionTime << "ms" << std::endl; + + out << "Trackers: " << std::endl; + for (const std::pair> &vvsIterData : timer.m_trackerVVSIterTimes) { + double trackingStartTime = timer.m_trackerIterStartTime.find(vvsIterData.first)->second; + double featTrackTime = timer.m_trackerFeatureTrackingTime.find(vvsIterData.first)->second; + double featExtractionTime = timer.m_trackerFeatureExtractionTime.find(vvsIterData.first)->second; + double initVVSTime = timer.m_trackerInitVVSTime.find(vvsIterData.first)->second; + + double ttVVSIter = 0.f; + for (double v : vvsIterData.second) { + ttVVSIter += v; + } + out << "\t" << vvsIterData.first << std::endl; + out << "\t" << "\t" << "Tracking initialization: " << trackingStartTime << "ms" << std::endl; + out << "\t" << "\t" << "Feature extraction: " << featExtractionTime << "ms" << std::endl; + out << "\t" << "\t" << "Feature tracking: " << featTrackTime << "ms" << std::endl; + out << "\t" << "\t" << "VVS init: " << initVVSTime << "ms" << std::endl; + out << "\t" << "\t" << "VVS: " << ttVVSIter << "ms (" << vpMath::getMean(vvsIterData.second) << "ms" + << "+-" << vpMath::getStdev(vvsIterData.second) << "ms)" << std::endl; + } + out << "====================================================" << std::endl; + out.flags(flags); + out << std::setprecision(default_precision); // restore defaults + return out; +} + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/include/visp3/rbt/vpRBVisualOdometry.h b/modules/tracker/rbt/include/visp3/rbt/vpRBVisualOdometry.h new file mode 100644 index 0000000000..37493a9802 --- /dev/null +++ b/modules/tracker/rbt/include/visp3/rbt/vpRBVisualOdometry.h @@ -0,0 +1,19 @@ +#ifndef VP_RB_VISUAL_ODOMETRY_H +#define VP_RB_VISUAL_ODOMETRY_H + +#include + +class vpRBFeatureTrackerInput; +class vpHomogeneousMatrix; + +class VISP_EXPORT vpRBVisualOdometry +{ +public: + vpRBVisualOdometry(); + virtual void compute(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame) = 0; + virtual vpHomogeneousMatrix getCameraMotion() const = 0; + virtual vpHomogeneousMatrix getCameraPose() const = 0; +}; + + +#endif diff --git a/modules/tracker/rbt/src/core/vpRBInitializationHelper.cpp b/modules/tracker/rbt/src/core/vpRBInitializationHelper.cpp new file mode 100644 index 0000000000..e15798f37b --- /dev/null +++ b/modules/tracker/rbt/src/core/vpRBInitializationHelper.cpp @@ -0,0 +1,304 @@ +#include + +#include +#include +#include +#include +#include + +#ifdef VISP_HAVE_MODULE_GUI +#include +#endif + +BEGIN_VISP_NAMESPACE + +void vpRBInitializationHelper::removeComment(std::ifstream &fileId) +{ + char c; + + fileId.get(c); + while (!fileId.fail() && (c == '#')) { + fileId.ignore(std::numeric_limits::max(), fileId.widen('\n')); + fileId.get(c); + } + if (fileId.fail()) { + throw(vpException(vpException::ioError, "Reached end of file")); + } + fileId.unget(); +} + +void vpRBInitializationHelper::savePose(const std::string &filename) const +{ + vpPoseVector init_pos; + std::fstream finitpos; + finitpos.open(filename.c_str(), std::ios::out); + + init_pos.buildFrom(m_cMo); + finitpos << init_pos; + finitpos.close(); +} + +#ifdef VISP_HAVE_MODULE_GUI + +template +void vpRBInitializationHelper::initClick(const vpImage &I, const std::string &initFile, bool displayHelp) +{ + std::cout << "Starting init click!" << std::endl; + vpHomogeneousMatrix last_cMo; + vpPoseVector init_pos; + vpImagePoint ip; + vpMouseButton::vpMouseButtonType button = vpMouseButton::button1; + + std::string ext = ".init"; + std::string str_pose = ""; + size_t pos = initFile.rfind(ext); + + // Load the last poses from files + std::fstream finitpos; + std::ifstream finit; + std::stringstream ss; + std::string poseSavingFilename; + if (poseSavingFilename.empty()) { + if (pos != std::string::npos) + str_pose = initFile.substr(0, pos) + ".0.pos"; + else + str_pose = initFile + ".0.pos"; + + finitpos.open(str_pose.c_str(), std::ios::in); + ss << str_pose; + } + else { + finitpos.open(poseSavingFilename.c_str(), std::ios::in); + ss << poseSavingFilename; + } + if (finitpos.fail()) { + std::cout << "Cannot read " << ss.str() << std::endl << "cMo set to identity" << std::endl; + last_cMo.eye(); + } + else { + for (unsigned int i = 0; i < 6; i += 1) { + finitpos >> init_pos[i]; + } + + finitpos.close(); + last_cMo.buildFrom(init_pos); + + std::cout << "Tracker initial pose read from " << ss.str() << ": " << std::endl << last_cMo << std::endl; + + vpDisplay::display(I); + vpDisplay::displayFrame(I, last_cMo, m_cam, 0.05, vpColor::green); + vpDisplay::flush(I); + + + std::cout << "No modification : left click " << std::endl; + std::cout << "Modify initial pose : right click " << std::endl; + + + vpDisplay::displayText(I, 15, 10, "left click to validate, right click to modify initial pose", vpColor::red); + + vpDisplay::flush(I); + + while (!vpDisplay::getClick(I, ip, button)) { + vpTime::sleepMs(10); + } + } + + if (!finitpos.fail() && button == vpMouseButton::button1) { + m_cMo = last_cMo; + } + else { + vpDisplay *d_help = nullptr; + + vpDisplay::display(I); + vpDisplay::flush(I); + + vpPose pose; + + pose.clearPoint(); + + // Clear string stream that previously contained the path to the "object.0.pos" file. + ss.str(std::string()); + + // file parser + // number of points + // X Y Z + // X Y Z + if (pos != std::string::npos) { + ss << initFile; + } + else { + ss << initFile; + ss << ".init"; + } + + std::cout << "Load 3D points from: " << ss.str() << std::endl; +#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) + finit.open(ss.str()); +#else + finit.open(ss.str().c_str()); +#endif + if (finit.fail()) { + std::cout << "Cannot read " << ss.str() << std::endl; + throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", ss.str().c_str()); + } + +#ifdef VISP_HAVE_MODULE_IO + // Display window creation and initialisation + try { + if (displayHelp) { + const std::string imgExtVec[] = { ".ppm", ".pgm", ".jpg", ".jpeg", ".png" }; + std::string dispF; + bool foundHelpImg = false; + if (pos != std::string::npos) { + for (size_t i = 0; i < 5 && !foundHelpImg; i++) { + dispF = initFile.substr(0, pos) + imgExtVec[i]; + foundHelpImg = vpIoTools::checkFilename(dispF); + } + } + else { + for (size_t i = 0; i < 5 && !foundHelpImg; i++) { + dispF = initFile + imgExtVec[i]; + foundHelpImg = vpIoTools::checkFilename(dispF); + } + } + + if (foundHelpImg) { + std::cout << "Load image to help initialization: " << dispF << std::endl; + + std::shared_ptr d_help = vpDisplayFactory::createDisplay(); + + vpImage Iref; + vpImageIo::read(Iref, dispF); +#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) + const int winXPos = I.display->getWindowXPosition(); + const int winYPos = I.display->getWindowYPosition(); + unsigned int width = I.getWidth(); + d_help->init(Iref, winXPos + (int)width + 80, winYPos, "Where to initialize..."); + vpDisplay::display(Iref); + vpDisplay::flush(Iref); +#endif + } + } + } + catch (...) { + if (d_help != nullptr) { + delete d_help; + d_help = nullptr; + } + } +#else //#ifdef VISP_HAVE_MODULE_IO + (void)(displayHelp); +#endif //#ifdef VISP_HAVE_MODULE_IO + // skip lines starting with # as comment + removeComment(finit); + + unsigned int n3d; + finit >> n3d; + finit.ignore(256, '\n'); // skip the rest of the line + std::cout << "Number of 3D points " << n3d << std::endl; + if (n3d > 100000) { + throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed", + ss.str().c_str()); + } + + std::vector P(n3d); + for (unsigned int i = 0; i < n3d; i++) { + // skip lines starting with # as comment + removeComment(finit); + + vpColVector pt_3d(4, 1.0); + finit >> pt_3d[0]; + finit >> pt_3d[1]; + finit >> pt_3d[2]; + finit.ignore(256, '\n'); // skip the rest of the line + + vpColVector pt_3d_tf = pt_3d; + std::cout << "Point " << i + 1 << " with 3D coordinates: " << pt_3d_tf[0] << " " << pt_3d_tf[1] << " " + << pt_3d_tf[2] << std::endl; + + P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]); // (X,Y,Z) + } + + finit.close(); + + bool isWellInit = false; + while (!isWellInit) { + std::vector mem_ip; + for (unsigned int i = 0; i < n3d; i++) { + std::ostringstream text; + text << "Click on point " << i + 1; + + vpDisplay::display(I); + vpDisplay::displayText(I, 15, 10, text.str(), vpColor::red); + for (unsigned int k = 0; k < mem_ip.size(); k++) { + vpDisplay::displayCross(I, mem_ip[k], 10, vpColor::green, 2); + } + vpDisplay::flush(I); + + std::cout << "Click on point " << i + 1 << " "; + double x = 0, y = 0; + + vpDisplay::getClick(I, ip); + mem_ip.push_back(ip); + vpDisplay::flush(I); + + vpPixelMeterConversion::convertPoint(m_cam, ip, x, y); + P[i].set_x(x); + P[i].set_y(y); + + std::cout << "with 2D coordinates: " << ip << std::endl; + + pose.addPoint(P[i]); // and added to the pose computation point list + } + vpDisplay::flush(I); + vpDisplay::display(I); + + std::cout << "Before optim: " << m_cam << std::endl; + if (!pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, m_cMo)) { + std::cout << "Pose computation from points failed!" << std::endl; + for (unsigned int i = 0; i < n3d; ++i) { + std::cout << "Point " << i << ": " << std::endl; + std::cout << " 3D: " << pose.getPoints()[i].get_oP().t() << std::endl; + std::cout << "2D: " << pose.getPoints()[i].get_x() << ", " << pose.getPoints()[i].get_y() << std::endl; + } + } + std::cout << "POSE after optim: " << m_cMo << std::endl; + + vpDisplay::displayText(I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red); + + vpDisplay::flush(I); + + button = vpMouseButton::button1; + while (!vpDisplay::getClick(I, ip, button)) { + } + + if (button == vpMouseButton::button1) { + isWellInit = true; + } + else { + pose.clearPoint(); + vpDisplay::display(I); + vpDisplay::flush(I); + } + + } + vpDisplay::displayFrame(I, m_cMo, m_cam, 0.05, vpColor::red); + + // save the pose into file + if (poseSavingFilename.empty()) + savePose(str_pose); + else + savePose(poseSavingFilename); + + if (d_help != nullptr) { + delete d_help; + d_help = nullptr; + } + } + +} +template void vpRBInitializationHelper::initClick(const vpImage &I, const std::string &initFile, bool displayHelp); +template void vpRBInitializationHelper::initClick(const vpImage &I, const std::string &initFile, bool displayHelp); +#endif + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp b/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp new file mode 100644 index 0000000000..f946d89b06 --- /dev/null +++ b/modules/tracker/rbt/src/core/vpRBSilhouetteControlPoint.cpp @@ -0,0 +1,518 @@ +/* + * 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 +#include +#include + +#define VISP_DEBUG_RB_CONTROL_POINT 1 + +BEGIN_VISP_NAMESPACE + +void vpRBSilhouetteControlPoint::init() +{ + m_valid = false; +} + +vpRBSilhouetteControlPoint::vpRBSilhouetteControlPoint() +{ + init(); + m_me = nullptr; + m_numCandidates = 1; + m_candidates.resize(1); + sign = 1; + norm.resize(3); + theta = 0; + m_isSilhouette = false; + m_valid = true; +} + +vpRBSilhouetteControlPoint::vpRBSilhouetteControlPoint(const vpRBSilhouetteControlPoint &meTracker) +{ + init(); + *this = meTracker; +} + +vpRBSilhouetteControlPoint &vpRBSilhouetteControlPoint::operator=(const vpRBSilhouetteControlPoint &meTracker) +{ + m_me = meTracker.m_me; + s = meTracker.s; + m_numCandidates = meTracker.m_numCandidates; + m_cam = meTracker.m_cam; + icpoint = meTracker.icpoint; + cpoint = meTracker.cpoint; + cpointo = meTracker.cpointo; + norm = meTracker.norm; + normw = meTracker.normw; + xs = meTracker.xs; + ys = meTracker.ys; + nxs = meTracker.nxs; + nys = meTracker.nys; + Zs = meTracker.Zs; + m_isSilhouette = meTracker.m_isSilhouette; + rho = meTracker.rho; + theta = meTracker.theta; + thetaInit = meTracker.thetaInit; + delta = meTracker.delta; + sign = meTracker.sign; + featureline = meTracker.featureline; + line = meTracker.line; + m_valid = meTracker.m_valid; + return *this; +} + +vpRBSilhouetteControlPoint::vpRBSilhouetteControlPoint(const vpRBSilhouetteControlPoint &&meTracker) +{ + *this = meTracker; +} + +vpRBSilhouetteControlPoint &vpRBSilhouetteControlPoint::operator=(const vpRBSilhouetteControlPoint &&meTracker) +{ + m_me = std::move(meTracker.m_me); + s = std::move(meTracker.s); + thetaInit = std::move(meTracker.thetaInit); + m_numCandidates = std::move(meTracker.m_numCandidates); + m_cam = std::move(meTracker.m_cam); + icpoint = std::move(meTracker.icpoint); + cpoint = std::move(meTracker.cpoint); + cpointo = std::move(meTracker.cpointo); + norm = std::move(meTracker.norm); + normw = std::move(meTracker.normw); + xs = std::move(meTracker.xs); + ys = std::move(meTracker.ys); + nxs = std::move(meTracker.nxs); + nys = std::move(meTracker.nys); + Zs = std::move(meTracker.Zs); + m_isSilhouette = std::move(meTracker.m_isSilhouette); + rho = std::move(meTracker.rho); + theta = std::move(meTracker.theta); + delta = std::move(meTracker.delta); + sign = std::move(meTracker.sign); + featureline = std::move(meTracker.featureline); + line = std::move(meTracker.line); + m_valid = std::move(meTracker.m_valid); + return *this; +} + +int vpRBSilhouetteControlPoint::outOfImage(int i, int j, int half, int rows, int cols) const +{ + return (!((i> half+2) && + (i< rows -(half+2)) && + (j>half+2) && + (j half+2) && + (i< rows -(half+2)) && + (j>half+2) && + (j &I) +{ + + if (s.getState() == vpMeSite::NO_SUPPRESSION) { + try { + if (s.m_convlt == 0) { + s.track(I, m_me, false); + } + else { + s.track(I, m_me, false); + } + } + catch (vpTrackingException &) { + vpERROR_TRACE("caught a tracking exception, ignoring me point..."); + s.setState(vpMeSite::THRESHOLD); + } + } +} + +void vpRBSilhouetteControlPoint::trackMultipleHypotheses(const vpImage &I) +{ + // If element hasn't been suppressed + try { + if (s.getState() == vpMeSite::NO_SUPPRESSION) { + //const bool testContrast = s.m_convlt != 0.0; + s.trackMultipleHypotheses(I, *m_me, false, m_candidates, m_numCandidates); + } + } + catch (vpTrackingException &) { + vpERROR_TRACE("caught a tracking exception, ignoring me point..."); + s.setState(vpMeSite::THRESHOLD); + } +} + +/*! + Build a 3D plane thanks the 3D coordinate of the control point and the normal vector to the surface + + \param[in] pointn : A point on the plane with coordinates in the object frame (oX, oY, oZ). + \param[in] normal : Normal of the plane. + \param[out] plane : The vpPlane instance used to store the computed plane equation. +*/ +void +vpRBSilhouetteControlPoint::buildPlane(const vpPoint &pointn, const vpColVector &normal, vpPlane &plane) +{ + plane.init(pointn, normal, vpPlane::object_frame); +} + +void +vpRBSilhouetteControlPoint::buildPLine(const vpHomogeneousMatrix &oMc) +{ + vpPlane plane; + vpPlane plane1; + vpPlane plane2; + buildPlane(cpoint, norm, plane); + vpRotationMatrix R; + oMc.extract(R); + + vpColVector V(3); + vpColVector Vo(3); + if (abs(theta) > 1e-2) { + V[0] = ((cpoint.get_oX()/cpoint.get_oZ())+1)*cpoint.get_oZ()-cpoint.get_oX(); + V[1] = ((cpoint.get_oY()/cpoint.get_oZ())-cos(theta)/sin(theta))*cpoint.get_oZ()-cpoint.get_oY(); + V[2] = (-plane.getD()-V[0]*plane.getA()-V[1]*plane.getB())/plane.getC()-cpoint.get_oZ(); + } + else { + V[0] = ((cpoint.get_oX()/cpoint.get_oZ())+1)*cpoint.get_oZ()-cpoint.get_oX(); + V[1] = ((cpoint.get_oY()/cpoint.get_oZ()))*cpoint.get_oZ()-cpoint.get_oY(); + V[2] = (-plane.getD()-V[0]*plane.getA()-V[1]*plane.getB())/plane.getC()-cpoint.get_oZ(); + } + + Vo = R*V; + vpColVector norm2 = vpColVector::cross(Vo, normw); + buildPlane(cpointo, norm2, plane2); + buildPlane(cpointo, normw, plane1); + + line.setWorldCoordinates(plane1.getA(), plane1.getB(), plane1.getC(), plane1.getD(), + plane2.getA(), plane2.getB(), plane2.getC(), plane2.getD()); +} + +void +vpRBSilhouetteControlPoint::buildPoint(int n, int m, const double &Z, double orient, const vpColVector &normo, + const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc, const vpCameraParameters &cam, const vpMe &me) +{ + m_cam = &cam; + m_me = &me; + + vpRotationMatrix R; + cMo.extract(R); + theta = orient; + thetaInit = theta; + double px = m_cam->get_px(); + double py = m_cam->get_py(); + int jc = m_cam->get_u0(); + int ic = m_cam->get_v0(); + icpoint.set_i(n); + icpoint.set_j(m); + double x, y; + x = (m-jc)/px; + y = (n-ic)/py; + rho = x*cos(theta)+y*sin(theta); + cpoint.setWorldCoordinates(x*Z, y*Z, Z); + cpoint.changeFrame(oMc); + cpointo.setWorldCoordinates(cpoint.get_X(), cpoint.get_Y(), cpoint.get_Z()); + normw = normo; + norm = R*normo; + nxs = cos(theta); + nys = sin(theta); + buildPLine(oMc); + m_valid = isLineDegenerate(); +} + +void +vpRBSilhouetteControlPoint::buildSilhouettePoint(int n, int m, const double &Z, double orient, const vpColVector &normo, + const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc, const vpCameraParameters &cam) +{ + m_cam = &cam; + vpRotationMatrix R; + cMo.extract(R); + theta = orient; + thetaInit = theta; + double px = m_cam->get_px(); + double py = m_cam->get_py(); + int jc = m_cam->get_u0(); + int ic = m_cam->get_v0(); + icpoint.set_i(n); + icpoint.set_j(m); + xs = (m-jc)/px; + ys = (n-ic)/py; + Zs = Z; + + nxs = cos(theta); + nys = sin(theta); + double x, y; + x = (m-jc)/px; + y = (n-ic)/py; + cpoint.setWorldCoordinates(x * Z, y * Z, Z); + cpoint.changeFrame(oMc); + cpointo.setWorldCoordinates(cpoint.get_X(), cpoint.get_Y(), cpoint.get_Z()); + normw = normo; + norm = R * normo; + buildPLine(oMc); +#if VISP_DEBUG_RB_CONTROL_POINT + if (std::isnan(line.getTheta())) { + std::cout << "Line in camera frame = " << line.cP << std::endl; + throw vpException(vpException::fatalError, "Incorrect line definition"); + + } +#endif + m_valid = isLineDegenerate(); +} + +void +vpRBSilhouetteControlPoint::update(const vpHomogeneousMatrix &_cMo) +{ + cpointo.changeFrame(_cMo); + cpointo.projection(); + double px = m_cam->get_px(); + double py = m_cam->get_py(); + double uc = m_cam->get_u0(); + double vc = m_cam->get_v0(); + double u, v; + v = py*cpointo.get_y()+vc; + u = px*cpointo.get_x()+uc; + icpoint.set_uv(u, v); +} + +void +vpRBSilhouetteControlPoint::updateSilhouettePoint(const vpHomogeneousMatrix &cMo) +{ + cpointo.changeFrame(cMo); + cpointo.projection(); + const double px = m_cam->get_px(); + const double py = m_cam->get_py(); + const double uc = m_cam->get_u0(); + const double vc = m_cam->get_v0(); + const double v = py * cpointo.get_y() + vc; + const double u = px * cpointo.get_x() + uc; + icpoint.set_uv(u, v); + xs = cpointo.get_x(); + ys = cpointo.get_y(); + Zs = cpointo.get_Z(); + if (m_valid) { + try { + line.changeFrame(cMo); + line.projection(); + } + catch (vpException &e) { + m_valid = false; + } + m_valid = !isLineDegenerate(); + if (m_valid) { + vpFeatureBuilder::create(featureline, line); + theta = featureline.getTheta(); +#if VISP_DEBUG_RB_CONTROL_POINT + if (std::isnan(theta)) { + throw vpException(vpException::fatalError, "Got nan theta in updateSilhouettePoint"); + } +#endif + if (fabs(theta - thetaInit) < M_PI / 2.0) { + nxs = cos(theta); + nys = sin(theta); + } + else { + nxs = -cos(theta); + nys = -sin(theta); + } + } + } +} + +void vpRBSilhouetteControlPoint::initControlPoint(const vpImage &I, double cvlt) +{ + double delta = theta; + s.init((double)icpoint.get_i(), (double)icpoint.get_j(), delta, cvlt, sign); + if (m_me != nullptr) { + const double marginRatio = m_me->getThresholdMarginRatio(); + const double convolution = s.convolution(I, m_me); + s.init((double)icpoint.get_i(), (double)icpoint.get_j(), delta, convolution, sign); + const double contrastThreshold = fabs(convolution) * marginRatio; + s.setContrastThreshold(contrastThreshold, *m_me); + } +} + +void vpRBSilhouetteControlPoint::detectSilhouette(const vpImage &I) +{ + unsigned int k = 0; + int range = 4; + double c = cos(theta); + double s = sin(theta); + for (int n = -range; n <= range; n++) { + unsigned int ii = static_cast(round(icpoint.get_i() + s * n)); + unsigned int jj = static_cast(round(icpoint.get_j() + c * n)); + unsigned int isBg = static_cast(I[ii][jj] == 0.f); + k += isBg; + } + m_isSilhouette = k > 2; +} + +/*! + Compute the interaction matrix and the error vector corresponding to the line. +*/ +void +vpRBSilhouetteControlPoint::computeMeInteractionMatrixError(const vpHomogeneousMatrix &cMo, unsigned int i, vpMatrix &L, vpColVector &e) +{ + line.changeFrame(cMo); + + m_valid = false; + if (!isLineDegenerate()) { + line.projection(); + vpFeatureBuilder::create(featureline, line); + + double rho0 = featureline.getRho(); + double theta0 = featureline.getTheta(); +#if VISP_DEBUG_RB_CONTROL_POINT + if (std::isnan(theta0)) { + std::cout << "Line in camera frame = " << line.cP.t() << std::endl; + std::cout << "Line in object frame = " << line.oP.t() << std::endl; + featureline.print(); + throw vpException(vpException::fatalError, "Got nan theta in computeInteractionMatrixError"); + } +#endif + double co = cos(theta0); + double si = sin(theta0); + + double mx = 1.0 / m_cam->get_px(); + double my = 1.0 / m_cam->get_py(); + double xc = m_cam->get_u0(); + double yc = m_cam->get_v0(); + + vpMatrix H; + H = featureline.interaction(); + double x = (double)s.m_j, y = (double)s.m_i; + + x = (x-xc)*mx; + y = (y-yc)*my; + + const double alpha = x*si - y*co; + + double *Lrho = H[0]; + double *Ltheta = H[1]; + // Calculate interaction matrix for a distance + for (unsigned int k = 0; k < 6; k++) { + L[i][k] = (Lrho[k] + alpha*Ltheta[k]); + } + e[i] = rho0 - (x*co + y*si); + m_valid = true; + } + else { + m_valid = false; + e[i] = 0; + for (unsigned int k = 0; k < 6; k++) { + L[i][k] = 0.0; + } + } +} + +void +vpRBSilhouetteControlPoint::computeMeInteractionMatrixErrorMH(const vpHomogeneousMatrix &cMo, unsigned int i, vpMatrix &L, vpColVector &e) +{ + line.changeFrame(cMo); + + m_valid = false; + if (!isLineDegenerate()) { + line.projection(); + + vpFeatureBuilder::create(featureline, line); + const double rho0 = featureline.getRho(); + const double theta0 = featureline.getTheta(); +#if VISP_DEBUG_RB_CONTROL_POINT + if (std::isnan(theta0)) { + throw vpException(vpException::fatalError, "Got nan theta in computeInteractionMatrixMH"); + } +#endif + + const double co = cos(theta0); + const double si = sin(theta0); + + const double mx = 1.0 / m_cam->get_px(); + const double my = 1.0 / m_cam->get_py(); + const double xc = m_cam->get_u0(); + const double yc = m_cam->get_v0(); + const vpMatrix &H = featureline.interaction(); + double xmin, ymin; + double errormin = std::numeric_limits::max(); + + const std::vector &cs = m_candidates; + xmin = (s.m_j - xc) * mx; + ymin = (s.m_i - yc) * my; + for (unsigned int l = 0; l < m_numCandidates; l++) //for each candidate of P + { + const vpMeSite &Pk = cs[l]; + + if ((Pk.getState() == vpMeSite::NO_SUPPRESSION)) { + const double x = (Pk.m_j - xc) * mx; + const double y = (Pk.m_i - yc) * my; + const double err = fabs(rho0 - (x * co + y * si)); + if (err <= errormin) { + errormin = err; + xmin = x; + ymin = y; + m_valid = true; + } + } + } + if (m_valid) { + e[i] = rho0 - (xmin * co + ymin * si); + const double alpha = xmin * si - ymin * co; + + const double *Lrho = H[0]; + const double *Ltheta = H[1]; + // Calculate interaction matrix for a distance + for (unsigned int k = 0; k < 6; k++) { + L[i][k] = (Lrho[k] + alpha * Ltheta[k]); + } + } + else { + e[i] = 0; + for (unsigned int k = 0; k < 6; k++) { + L[i][k] = 0.0; + } + } + } +} + +bool vpRBSilhouetteControlPoint::isLineDegenerate() const +{ + double a, b, d; + a = line.cP[4] * line.cP[3] - line.cP[0] * line.cP[7]; + b = line.cP[5] * line.cP[3] - line.cP[1] * line.cP[7]; + d = a*a + b*b; + return d <= 1e-7; +} + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/core/vpRBSilhouettePointsExtractionSettings.cpp b/modules/tracker/rbt/src/core/vpRBSilhouettePointsExtractionSettings.cpp new file mode 100644 index 0000000000..21d07af113 --- /dev/null +++ b/modules/tracker/rbt/src/core/vpRBSilhouettePointsExtractionSettings.cpp @@ -0,0 +1,132 @@ + +/* + * 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 +#include +#include +#include +#include + +#include +#include + +BEGIN_VISP_NAMESPACE + +vpSilhouettePointsExtractionSettings::vpSilhouettePointsExtractionSettings() +{ + m_depthThreshold = 0.1; + m_thresholdIsRelative = false; + m_preferPreviousPoints = false; + m_sampleStep = 5; + m_maxNumPoints = 0; + m_border = 10; +} + +vpSilhouettePointsExtractionSettings::vpSilhouettePointsExtractionSettings(const vpSilhouettePointsExtractionSettings &rend) +{ + *this = rend; +} + +const vpSilhouettePointsExtractionSettings &vpSilhouettePointsExtractionSettings::operator=(const vpSilhouettePointsExtractionSettings &rend) +{ + m_depthThreshold = rend.m_depthThreshold; + m_thresholdIsRelative = rend.m_thresholdIsRelative; + m_sampleStep = rend.m_sampleStep; + m_maxNumPoints = rend.m_maxNumPoints; + m_preferPreviousPoints = rend.m_preferPreviousPoints; + m_border = rend.m_border; + return *this; +} + +std::vector> vpSilhouettePointsExtractionSettings::getSilhouetteCandidates( + const vpImage &validSilhouette, const vpImage &renderDepth, + const vpCameraParameters &cam, const vpHomogeneousMatrix &cTcp, + const std::vector &previousPoints, long randomSeed) const +{ + const unsigned int rows = validSilhouette.getHeight(); + const unsigned int cols = validSilhouette.getWidth(); + + std::vector> finalCandidates; + std::vector> candidates; + if (m_maxNumPoints) { + finalCandidates.reserve(m_maxNumPoints); + candidates.reserve(m_maxNumPoints); + } + if (m_preferPreviousPoints) { + for (const vpRBSilhouettePoint &p: previousPoints) { + double x = 0.0, y = 0.0; + vpPixelMeterConversion::convertPoint(cam, p.j, p.i, x, y); + vpColVector cpX({ x * p.Z, y * p.Z, p.Z, 1.0 }); + vpColVector cX = cTcp * cpX; + cX /= cX[3]; + vpMeterPixelConversion::convertPoint(cam, cX[0] / cX[2], cX[1] / cX[2], x, y); + + unsigned nu = static_cast(round(x)), nv = static_cast(round(y)); + if (nu > 0 && nv > 0 && nv < rows && nu < cols) { + if (validSilhouette[nv][nu] > 0 && fabs((renderDepth[nv][nu] / p.Z) - 1.0) < 0.01) { + finalCandidates.push_back(std::make_pair(nv, nu)); + } + } + } + } + if (m_maxNumPoints > 0 && finalCandidates.size() >= static_cast(m_maxNumPoints)) { + return finalCandidates; + } + + for (unsigned int n = m_border; n < rows - m_border; n += m_sampleStep) { + for (unsigned int m = m_border; m < cols - m_border; m += m_sampleStep) { + //std::cout << "n = " << n << ", m = " << m << ", h = " << rows << ", w = " << cols << std::endl; + //std::cout << "m = " << m << ", n = " << n << ", s = " << (int)(validSilhouette[n][m]) << std::endl; + if (validSilhouette[n][m] > 0) { + candidates.push_back(std::make_pair(n, m)); + } + } + } + + if (m_maxNumPoints > 0) { + vpUniRand random(randomSeed); + std::vector indices(m_maxNumPoints - finalCandidates.size()); + sampleWithoutReplacement(m_maxNumPoints - finalCandidates.size(), candidates.size(), indices, random); + for (unsigned int i = 0; i < indices.size(); ++i) { + finalCandidates.push_back(candidates[indices[i]]); + } + } + else { + for (unsigned int i = 0; i < candidates.size(); ++i) { + finalCandidates.push_back(candidates[i]); + } + } + return finalCandidates; +} + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/core/vpRBTracker.cpp b/modules/tracker/rbt/src/core/vpRBTracker.cpp new file mode 100644 index 0000000000..dc250cf88d --- /dev/null +++ b/modules/tracker/rbt/src/core/vpRBTracker.cpp @@ -0,0 +1,677 @@ +/* + * 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_NLOHMANN_JSON) +#include VISP_NLOHMANN_JSON(json.hpp) +#endif + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +BEGIN_VISP_NAMESPACE + +vpRBTracker::vpRBTracker() : + m_firstIteration(true), m_trackers(0), m_lambda(1.0), m_vvsIterations(10), m_muInit(0.0), m_muIterFactor(0.5), + m_renderer(m_rendererSettings), m_imageHeight(480), m_imageWidth(640), m_verbose(false) +{ + m_rendererSettings.setClippingDistance(0.01, 1.0); + m_renderer.setRenderParameters(m_rendererSettings); + + m_driftDetector = nullptr; + m_mask = nullptr; + m_odometry = nullptr; +} + +void vpRBTracker::getPose(vpHomogeneousMatrix &cMo) const +{ + cMo = m_cMo; +} + +void vpRBTracker::setPose(const vpHomogeneousMatrix &cMo) +{ + m_cMo = cMo; + m_cMoPrev = cMo; + m_renderer.setCameraPose(cMo.inverse()); +} + +vpMatrix vpRBTracker::getCovariance() const +{ + vpMatrix sumInvs(6, 6, 0.0); + double sumWeights = 0.0; + for (const std::shared_ptr &tracker: m_trackers) { + if (tracker->getNumFeatures() == 0) { + continue; + } + tracker->updateCovariance(m_lambda); + vpMatrix trackerCov = tracker->getCovariance(); + double trackerWeight = tracker->getVVSTrackerWeight(); + if (trackerCov.getRows() != 6 || trackerCov.getCols() != 6) { + throw vpException(vpException::dimensionError, + "Expected tracker pose covariance to have dimensions 6x6, but got %dx%d", + trackerCov.getRows(), trackerCov.getCols()); + } + + sumInvs += (trackerWeight * trackerCov.pseudoInverse()); + sumWeights += trackerWeight; + } + return sumWeights * sumInvs.pseudoInverse(); +} + +vpCameraParameters vpRBTracker::getCameraParameters() const { return m_cam; } + +void vpRBTracker::setCameraParameters(const vpCameraParameters &cam, unsigned h, unsigned w) +{ + if (cam.get_projModel() != vpCameraParameters::vpCameraParametersProjType::perspectiveProjWithoutDistortion) { + throw vpException(vpException::badValue, + "Camera model cannot have distortion. Undistort images before tracking and use the undistorted camera model"); + } + if (h == 0 || w == 0) { + throw vpException( + vpException::badValue, + "Image dimensions must be greater than 0" + ); + } + m_cam = cam; + m_imageHeight = h; + m_imageWidth = w; + m_rendererSettings.setCameraIntrinsics(m_cam); + m_rendererSettings.setImageResolution(m_imageHeight, m_imageWidth); + m_renderer.setRenderParameters(m_rendererSettings); +} + +void vpRBTracker::setSilhouetteExtractionParameters(const vpSilhouettePointsExtractionSettings &settings) +{ + m_depthSilhouetteSettings = settings; +} + +void vpRBTracker::reset() +{ + m_firstIteration = true; +} + +void vpRBTracker::setModelPath(const std::string &path) +{ + m_modelPath = path; +} + +void vpRBTracker::setupRenderer(const std::string &file) +{ + if (!vpIoTools::checkFilename(file)) { + throw vpException(vpException::badValue, "3D model file %s could not be found", file.c_str()); + } + + const std::shared_ptr geometryRenderer = std::make_shared( + vpPanda3DGeometryRenderer::vpRenderType::OBJECT_NORMALS); + m_renderer.addSubRenderer(geometryRenderer); + + bool requiresSilhouetteShader = false; + for (std::shared_ptr &tracker: m_trackers) { + if (tracker->requiresSilhouetteCandidates()) { + requiresSilhouetteShader = true; + break; + } + } + if (requiresSilhouetteShader) { + m_renderer.addSubRenderer(std::make_shared( + "depthCanny", geometryRenderer, true, 0.0)); + } + m_renderer.initFramework(); + m_renderer.addLight(vpPanda3DAmbientLight("ambient", vpRGBf(0.4f))); + m_renderer.addNodeToScene(m_renderer.loadObject("object", file)); + m_renderer.setFocusedObject("object"); +} + +void vpRBTracker::track(const vpImage &I) +{ + for (std::shared_ptr tracker : m_trackers) { + if (tracker->requiresDepth() || tracker->requiresRGB()) { + throw vpException(vpException::badValue, "Some tracked features require RGB or depth features"); + } + } + checkDimensionsOrThrow(I, "grayscale"); + vpRBFeatureTrackerInput frameInput; + frameInput.I = I; + frameInput.cam = m_cam; + track(frameInput); +} + +void vpRBTracker::track(const vpImage &I, const vpImage &IRGB) +{ + for (std::shared_ptr &tracker : m_trackers) { + if (tracker->requiresDepth()) { + throw vpException(vpException::badValue, "Some tracked features require depth features"); + } + } + checkDimensionsOrThrow(I, "grayscale"); + checkDimensionsOrThrow(IRGB, "color"); + vpRBFeatureTrackerInput frameInput; + frameInput.I = I; + frameInput.IRGB = IRGB; + frameInput.cam = m_cam; + track(frameInput); +} + +void vpRBTracker::startTracking() +{ + setupRenderer(m_modelPath); +} + +void vpRBTracker::track(const vpImage &I, const vpImage &IRGB, const vpImage &depth) +{ + checkDimensionsOrThrow(I, "grayscale"); + checkDimensionsOrThrow(IRGB, "color"); + checkDimensionsOrThrow(depth, "depth"); + vpRBFeatureTrackerInput frameInput; + frameInput.I = I; + frameInput.IRGB = IRGB; + frameInput.depth = depth; + frameInput.cam = m_cam; + track(frameInput); +} + +void vpRBTracker::track(vpRBFeatureTrackerInput &input) +{ + m_logger.reset(); + + m_logger.startTimer(); + updateRender(input); + m_logger.setRenderTime(m_logger.endTimer()); + + if (m_firstIteration) { + m_firstIteration = false; + m_previousFrame.I = input.I; + m_previousFrame.IRGB = input.IRGB; + } + + m_logger.startTimer(); + if (m_mask) { + m_mask->updateMask(input, m_previousFrame, input.mask); + } + m_logger.setMaskTime(m_logger.endTimer()); + + bool requiresSilhouetteCandidates = false; + for (std::shared_ptr &tracker : m_trackers) { + if (tracker->requiresSilhouetteCandidates()) { + requiresSilhouetteCandidates = true; + break; + } + } + + m_logger.startTimer(); + if (requiresSilhouetteCandidates) { + const vpHomogeneousMatrix cTcp = m_cMo * m_cMoPrev.inverse(); + input.silhouettePoints = extractSilhouettePoints(input.renders.normals, input.renders.depth, + input.renders.silhouetteCanny, input.renders.isSilhouette, input.cam, cTcp); + if (input.silhouettePoints.size() == 0) { + throw vpException(vpException::badValue, "Could not extract silhouette from depth canny: Object may not be in image"); + } + } + + if (m_odometry) { + m_logger.startTimer(); + m_odometry->compute(input, m_previousFrame); + vpHomogeneousMatrix cnTc = m_odometry->getCameraMotion(); + m_cMo = cnTc * m_cMo; + updateRender(input); + m_logger.setOdometryTime(m_logger.endTimer()); + } + + if (requiresSilhouetteCandidates) { + const vpHomogeneousMatrix cTcp = m_cMo * m_cMoPrev.inverse(); + input.silhouettePoints = extractSilhouettePoints(input.renders.normals, input.renders.depth, + input.renders.silhouetteCanny, input.renders.isSilhouette, input.cam, cTcp); + if (input.silhouettePoints.size() == 0) { + throw vpException(vpException::badValue, "Could not extract silhouette from depth canny: Object may not be in image"); + } + } + + + int id = 0; + for (std::shared_ptr &tracker : m_trackers) { + m_logger.startTimer(); + tracker->onTrackingIterStart(); + m_logger.setTrackerIterStartTime(id, m_logger.endTimer()); + id += 1; + } + id = 0; + for (std::shared_ptr &tracker : m_trackers) { + m_logger.startTimer(); + try { + tracker->extractFeatures(input, m_previousFrame, m_cMo); + } + catch (vpException &e) { + std::cerr << "Tracker " << id << " raised an exception in extractFeatures" << std::endl; + throw e; + } + m_logger.setTrackerFeatureExtractionTime(id, m_logger.endTimer()); + id += 1; + } + id = 0; + for (std::shared_ptr &tracker : m_trackers) { + m_logger.startTimer(); + try { + tracker->trackFeatures(input, m_previousFrame, m_cMo); + } + catch (vpException &e) { + std::cerr << "Tracker " << id << " raised an exception in trackFeatures" << std::endl; + throw e; + } + m_logger.setTrackerFeatureTrackingTime(id, m_logger.endTimer()); + id += 1; + } + + + + id = 0; + for (std::shared_ptr &tracker : m_trackers) { + m_logger.startTimer(); + tracker->initVVS(input, m_previousFrame, m_cMo); + m_logger.setInitVVSTime(id, m_logger.endTimer()); + //std::cout << "Tracker " << id << " has " << tracker->getNumFeatures() << " features" << std::endl; + id += 1; + } + + m_cMoPrev = m_cMo; + double bestError = std::numeric_limits::max(); + vpHomogeneousMatrix best_cMo = m_cMo; + double mu = m_muInit; + for (unsigned int iter = 0; iter < m_vvsIterations; ++iter) { + id = 0; + for (std::shared_ptr &tracker : m_trackers) { + m_logger.startTimer(); + try { + tracker->computeVVSIter(input, m_cMo, iter); + } + catch (vpException &) { + std::cerr << "Tracker " << id << " raised an exception in computeVVSIter" << std::endl; + throw; + } + m_logger.addTrackerVVSTime(id, m_logger.endTimer()); + id += 1; + } + + //! Check if all trackers have converged + bool converged = true; + for (std::shared_ptr &tracker : m_trackers) { + if (!tracker->vvsHasConverged()) { + converged = false; + break; + } + } + if (converged) { + break; + } + + vpMatrix LTL(6, 6, 0.0); + vpColVector LTR(6, 0.0); + double error = 0.f; + unsigned int numFeatures = 0; + + for (std::shared_ptr &tracker : m_trackers) { + if (tracker->getNumFeatures() > 0) { + numFeatures += tracker->getNumFeatures(); + const double weight = tracker->getVVSTrackerWeight(); + LTL += weight * tracker->getLTL(); + LTR += weight * tracker->getLTR(); + error += (weight * tracker->getWeightedError()).sumSquare(); + //std::cout << "Error = " << (weight * tracker->getWeightedError()).sumSquare() << std::endl; + } + } + + if (numFeatures >= 6) { + + if (error < bestError) { + bestError = error; + best_cMo = m_cMo; + } + + vpMatrix H(6, 6); + H.eye(6); + try { + vpColVector v = -m_lambda * ((LTL + mu * H).pseudoInverse(LTL.getRows() * std::numeric_limits::epsilon()) * LTR); + m_cMo = vpExponentialMap::direct(v).inverse() * m_cMo; + } + catch (vpException &) { + std::cerr << "Could not compute pseudo inverse" << std::endl; + } + mu *= m_muIterFactor; + } + else { + return; + } + } + + //m_cMo = best_cMo; + + for (std::shared_ptr &tracker : m_trackers) { + tracker->onTrackingIterEnd(); + } + //m_cMo = m_kalman.filter(m_cMo, 1.0 / 20.0); + if (m_currentFrame.I.getSize() == 0) { + m_currentFrame = input; + m_previousFrame = input; + } + else { + m_previousFrame = std::move(m_currentFrame); + m_currentFrame = std::move(input); + } + m_logger.startTimer(); + if (m_driftDetector) { + m_driftDetector->update(m_previousFrame, m_currentFrame, m_cMo, m_cMoPrev); + } + m_logger.setDriftDetectionTime(m_logger.endTimer()); + if (m_verbose) { + std::cout << m_logger << std::endl; + } +} + +void vpRBTracker::updateRender(vpRBFeatureTrackerInput &frame) +{ + m_renderer.setCameraPose(m_cMo.inverse()); + + frame.renders.cMo = m_cMo; + + // Update clipping distances + frame.renders.normals.resize(m_imageHeight, m_imageWidth); + frame.renders.silhouetteCanny.resize(m_imageHeight, m_imageWidth); + frame.renders.isSilhouette.resize(m_imageHeight, m_imageWidth); + + float clipNear, clipFar; + m_renderer.computeClipping(clipNear, clipFar); + frame.renders.zNear = std::max(0.001f, clipNear); + frame.renders.zFar = clipFar; + m_rendererSettings.setClippingDistance(frame.renders.zNear, frame.renders.zFar); + m_renderer.setRenderParameters(m_rendererSettings); + + bool shouldRenderSilhouette = m_renderer.getRenderer() != nullptr; + if (shouldRenderSilhouette) { + // For silhouette extraction, update depth difference threshold + double thresholdValue = m_depthSilhouetteSettings.getThreshold(); + if (m_depthSilhouetteSettings.thresholdIsRelative()) { + m_renderer.getRenderer()->setEdgeThreshold((frame.renders.zFar - frame.renders.zNear) * thresholdValue); + } + else { + m_renderer.getRenderer()->setEdgeThreshold(thresholdValue); + } + } + + // Call Panda renderer + m_renderer.renderFrame(); + + frame.renders.boundingBox = m_renderer.getBoundingBox(); + + // Extract data from Panda textures +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel sections +#endif + + { +#ifdef VISP_HAVE_OPENMP +#pragma omp section +#endif + { + m_renderer.getRenderer()->getRender( + frame.renders.normals, + frame.renders.depth, + frame.renders.boundingBox, + m_imageHeight, m_imageWidth); + } +#ifdef VISP_HAVE_OPENMP +#pragma omp section +#endif + { + if (shouldRenderSilhouette) { + m_renderer.getRenderer()->getRender( + frame.renders.silhouetteCanny, + frame.renders.isSilhouette, + frame.renders.boundingBox, + m_imageHeight, m_imageWidth); + } + } + // #pragma omp section + // { + // vpImage renders.color; + // m_renderer.getRenderer()->getRender(renders.color); + // m_renderer.placeRendernto(renders.color, frame.renders.color, vpRGBa(0)); + // } + } + +} + +std::vector vpRBTracker::extractSilhouettePoints( + const vpImage &Inorm, const vpImage &Idepth, + const vpImage &silhouetteCanny, const vpImage &Ivalid, + const vpCameraParameters &cam, const vpHomogeneousMatrix &cTcp) +{ + std::vector> candidates = + m_depthSilhouetteSettings.getSilhouetteCandidates(Ivalid, Idepth, cam, cTcp, m_previousFrame.silhouettePoints, 42); + + std::vector points; + vpColVector norm(3); + + for (unsigned int i = 0; i < candidates.size(); ++i) { + unsigned int n = candidates[i].first, m = candidates[i].second; + double theta = silhouetteCanny[n][m].B; + if (std::isnan(theta)) { + continue; + } + + norm[0] = Inorm[n][m].R; + norm[1] = Inorm[n][m].G; + norm[2] = Inorm[n][m].B; + const double l = std::sqrt(norm[0] * norm[0] + norm[1] * norm[1] + norm[2] * norm[2]); + + if (l > 1e-1) { + const double Z = Idepth[n][m]; + //bool noNeighbor = true; + // double nx = cos(theta); + // double ny = sin(theta); + // const double Zn = Idepth[static_cast(round(n + ny * 1))][static_cast(round(m + nx * 2))]; +#if defined(VISP_DEBUG_RB_TRACKER) + if (fabs(theta) > M_PI + 1e-6) { + throw vpException(vpException::badValue, "Theta expected to be in -Pi, Pi range but was not"); + } +#endif + points.push_back(vpRBSilhouettePoint(n, m, norm, theta, Z)); + // if (Zn > 0) { + // theta = -theta; + // } + // Code to filter when two edges are too close and should not be used + // for (unsigned int normalOffset = 1; normalOffset <= 3; ++normalOffset) { + // unsigned char offY = static_cast(round(n + normalOffset * ny)); + // unsigned char offX = static_cast(round(m + normalOffset * nx)); + // unsigned char negOffY = static_cast(round(n - normalOffset * ny)); + // unsigned char negOffX = static_cast(round(m - normalOffset * nx)); + // if (offY == n || offX == m || negOffY == n||negOffX == m) { + // continue; + // } + + // if (Ivalid(offY, offX) || Ivalid(negOffY, negOffX)) { + // noNeighbor = false; + // // std::cout << (unsigned int)(Ivalid(n + normalOffset * ny, m + normalOffset * nx)) << std::endl; + // break; + // } + // } + // if (noNeighbor) { + // points.push_back(vpRBSilhouettePoint(n, m, norm, theta, Z)); + // } + } + } + + return points; +} + +void vpRBTracker::addTracker(std::shared_ptr tracker) +{ + if (tracker == nullptr) { + throw vpException(vpException::badValue, "Adding tracker: tracker cannot be null"); + } + m_trackers.push_back(tracker); +} + +void vpRBTracker::displayMask(vpImage &Imask) const +{ + if (m_mask) { + m_mask->display(m_currentFrame.mask, Imask); + } +} + +void vpRBTracker::display(const vpImage &I, const vpImage &IRGB, const vpImage &depth) +{ + if (m_currentFrame.renders.normals.getSize() == 0) { + return; + } + + + for (std::shared_ptr &tracker : m_trackers) { + if (tracker->featuresShouldBeDisplayed()) { + tracker->display(m_currentFrame.cam, I, IRGB, depth); + } + } + + if (m_driftDetector) { + m_driftDetector->display(IRGB); + } +} + +vpObjectCentricRenderer &vpRBTracker::getRenderer() +{ + return m_renderer; +} + +#if defined(VISP_HAVE_NLOHMANN_JSON) +void vpRBTracker::loadConfigurationFile(const std::string &filename) +{ + std::ifstream jsonFile(filename); + if (!jsonFile.good()) { + throw vpException(vpException::ioError, "Could not read from settings file " + filename + " to initialize the RBTracker"); + } + nlohmann::json settings; + try { + settings = nlohmann::json::parse(jsonFile); + } + catch (nlohmann::json::parse_error &e) { + std::stringstream msg; + msg << "Could not parse JSON file : \n"; + msg << e.what() << std::endl; + msg << "Byte position of error: " << e.byte; + throw vpException(vpException::ioError, msg.str()); + } + loadConfiguration(settings); + jsonFile.close(); +} + +void vpRBTracker::loadConfiguration(const nlohmann::json &j) +{ + m_firstIteration = true; + const nlohmann::json verboseSettings = j.at("verbose"); + m_verbose = verboseSettings.value("enabled", m_verbose); + + const nlohmann::json cameraSettings = j.at("camera"); + m_cam = cameraSettings.at("intrinsics"); + m_imageHeight = cameraSettings.value("height", m_imageHeight); + m_imageWidth = cameraSettings.value("width", m_imageWidth); + setCameraParameters(m_cam, m_imageHeight, m_imageWidth); + + if (j.contains("model")) { + setModelPath(j.at("model")); + } + + const nlohmann::json vvsSettings = j.at("vvs"); + setMaxOptimizationIters(vvsSettings.value("maxIterations", m_vvsIterations)); + setOptimizationGain(vvsSettings.value("gain", m_lambda)); + setOptimizationInitialMu(vvsSettings.value("mu", m_muInit)); + setOptimizationMuIterFactor(vvsSettings.value("muIterFactor", m_muIterFactor)); + + m_depthSilhouetteSettings = j.at("silhouetteExtractionSettings"); + + m_trackers.clear(); + const nlohmann::json features = j.at("features"); + vpRBFeatureTrackerFactory &featureFactory = vpRBFeatureTrackerFactory::getFactory(); + for (const nlohmann::json &trackerSettings: features) { + std::shared_ptr tracker = featureFactory.buildFromJson(trackerSettings); + if (tracker == nullptr) { + throw vpException( + vpException::badValue, + "Cannot instantiate subtracker with the current settings, make sure that the type is registered. Settings: %s", + trackerSettings.dump(2).c_str() + ); + } + m_trackers.push_back(tracker); + } + + if (j.contains("mask")) { + vpObjectMaskFactory &maskFactory = vpObjectMaskFactory::getFactory(); + const nlohmann::json maskSettings = j.at("mask"); + m_mask = maskFactory.buildFromJson(maskSettings); + if (m_mask == nullptr) { + throw vpException( + vpException::badValue, + "Cannot instantiate object mask with the current settings, make sure that the type is registered. Settings: %s", + maskSettings.dump(2).c_str()); + } + } + if (j.contains("drift")) { + vpRBDriftDetectorFactory &factory = vpRBDriftDetectorFactory::getFactory(); + const nlohmann::json driftSettings = j.at("drift"); + m_driftDetector = factory.buildFromJson(driftSettings); + if (m_driftDetector == nullptr) { + throw vpException( + vpException::badValue, + "Cannot instantiate drift detection with the current settings, make sure that the type is registered in the factory" + ); + } + } +} +#endif + +#ifdef VISP_HAVE_MODULE_GUI +void vpRBTracker::initClick(const vpImage &I, const std::string &initFile, bool displayHelp) +{ + vpRBInitializationHelper initializer; + initializer.setCameraParameters(m_cam); + initializer.initClick(I, initFile, displayHelp); + m_cMo = initializer.getPose(); +} +#endif + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/drift/vpRBDriftDetectorFactory.cpp b/modules/tracker/rbt/src/drift/vpRBDriftDetectorFactory.cpp new file mode 100644 index 0000000000..392008cf89 --- /dev/null +++ b/modules/tracker/rbt/src/drift/vpRBDriftDetectorFactory.cpp @@ -0,0 +1,49 @@ +/* + * 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 + +BEGIN_VISP_NAMESPACE + +vpRBDriftDetectorFactory::vpRBDriftDetectorFactory() +{ + setJsonKeyFinder([](const nlohmann::json &j) -> std::string { + return j.at("type"); + }); + + registerType("probabilistic", [](const nlohmann::json &j) { + std::shared_ptr p(new vpRBProbabilistic3DDriftDetector()); + p->loadJsonConfiguration(j); + return p; + }); +} + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp b/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp new file mode 100644 index 0000000000..65cb50fae5 --- /dev/null +++ b/modules/tracker/rbt/src/drift/vpRBProbabilistic3DDriftDetector.cpp @@ -0,0 +1,253 @@ +/* + * 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 +#include +#include + +#include + +#if defined(VISP_HAVE_NLOHMANN_JSON) +#include VISP_NLOHMANN_JSON(json.hpp) +#endif + +BEGIN_VISP_NAMESPACE + +void vpRBProbabilistic3DDriftDetector::update(const vpRBFeatureTrackerInput &previousFrame, + const vpRBFeatureTrackerInput &frame, + const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &/*cprevTo*/) +{ + const vpHomogeneousMatrix &cprevTo = frame.renders.cMo; + const vpTranslationVector t = cprevTo.inverse().getTranslationVector(); + + if (m_points.size() > 0) { + // Step 0: project all points +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel for +#endif + for (vpStored3DSurfaceColorPoint &p : m_points) { + p.update(cTo, cprevTo, frame.cam); + } + + // Step 1: gather points visible in both images and in render + std::vector visiblePoints; + for (vpStored3DSurfaceColorPoint &p : m_points) { + p.visible = true; + if ( + p.projPrevPx[0] < 2 || static_cast(p.projPrevPx[0]) >= frame.IRGB.getWidth() - 2 + || p.projPrevPx[1] < 2 || static_cast(p.projPrevPx[1]) >= frame.IRGB.getHeight() - 2 + || p.projCurrPx[0] < 2 || static_cast(p.projCurrPx[0]) >= frame.IRGB.getWidth() - 2 + || p.projCurrPx[1] < 2 || static_cast(p.projCurrPx[1]) >= frame.IRGB.getHeight() - 2) { + p.visible = false; // Point is outside of either current or previous image, ignore it + continue; + } + + // if (fabs(p.currX[2] - ZcurrMap) > m_maxError3D || fabs(p.prevX[2] - ZprevMap) > m_maxError3D) { + // continue; // Depth is wrong: probable occlusion, ignore it + // } + + float ZrenderMap = frame.renders.depth[p.projPrevPx[1]][p.projPrevPx[0]]; + // Version 2: compare previous projection with render, this does not filter occlusions + if (ZrenderMap == 0.f || fabs(p.prevX[2] - ZrenderMap) > m_maxError3D) { + p.visible = false; + continue; + } + + vpRGBf normalObject = frame.renders.normals[p.projPrevPx[1]][p.projPrevPx[0]]; + + vpColVector cameraRay({ t[0] - p.X[0], t[1] - p.X[1], t[2] - p.X[2] }); + + cameraRay.normalize(); + double angle = acos(vpColVector::dotProd(vpColVector({ normalObject.R, normalObject.G, normalObject.B }).normalize(), cameraRay)); + if (angle > vpMath::rad(75)) { + p.visible = false; + continue; + } + + // Filter points that are too close to the silhouette edges + if (frame.silhouettePoints.size() > 0) { + for (const vpRBSilhouettePoint &sp: frame.silhouettePoints) { + if (std::pow(static_cast(sp.i) - p.projPrevPx[1], 2) + std::pow(static_cast(sp.j) - p.projPrevPx[0], 2) < vpMath::sqr(3)) { + p.visible = false; + break; + } + } + } + // Version 3: could be using version 1 and 2. If 1 is wrong but 2 is ok, then there is an issue that is not self occlusion + // We could reweigh the error by the number of problematic points + // ... + + if (p.visible) { + visiblePoints.push_back(&p); + } + } + + if (visiblePoints.size() > 0) { + // // Compute sample weight + // double maxTrace = 0.0; + + // for (vpStored3DSurfaceColorPoint *p : visiblePoints) { + // double trace = p->stats.trace(); + // if (trace > maxTrace) { + // maxTrace = trace; + // } + // } + // maxTrace = std::max(maxTrace, 80.0); + double weightSum = 0.0; + m_score = 0.0; + for (vpStored3DSurfaceColorPoint *p : visiblePoints) { + double maxProba = 0.0; + vpRGBf minColor; + const bool hasCorrectDepth = frame.hasDepth() && frame.depth[p->projPrevPx[1]][p->projPrevPx[0]] > 0.f; + const double Z = hasCorrectDepth ? frame.depth[p->projPrevPx[1]][p->projPrevPx[0]] : 0.0; + double depthError = Z > 0 ? fabs(p->prevX[2] - Z) : 0.0; + double probaDepth = 1.0; + if (hasCorrectDepth) { + probaDepth = 1.0 - erf((depthError) / (m_depthSigma * sqrt(2.0))); + } + // double weight = 1.0 - ((p->stats.trace() / maxTrace)); + // if (weight < 0.0) { + // throw vpException(vpException::badValue, "Got invalid weight"); + // } + double weight = 1.0; + + vpRGBf averageColor(0.f, 0.f, 0.f); + for (int i = -1; i < 2; ++i) { + for (int j = -1; j < 2; ++j) { + const vpRGBa currentColor = frame.IRGB[p->projCurrPx[1] + i][p->projCurrPx[0] + j]; + averageColor.R += currentColor.R; + averageColor.G += currentColor.G; + averageColor.B += currentColor.B; + } + } + averageColor = averageColor * (1.0 / 9.0); + // const vpRGBf c(currentColor.R, currentColor.G, currentColor.B); + const vpRGBf c(averageColor); + + const double probaColor = p->stats.probability(c); + const double proba = probaColor * probaDepth; + if (probaDepth > 1.f || probaDepth < 0.0) { + throw vpException(vpException::badValue, "Wrong depth probability"); + } + if (proba > maxProba) { + maxProba = proba; + minColor = c; + } + + m_score += maxProba * weight; + weightSum += weight; + p->updateColor(minColor, m_colorUpdateRate * probaDepth); + } + m_score /= (weightSum); + } + else { + m_score = 1.0; + } + } + else { + m_score = 1.0; + } + + // Step 4: Sample bb to add new visible points + const vpHomogeneousMatrix oMcprev = cprevTo.inverse(); + vpColVector cX(4, 1.0); + vpColVector oX(4, 1.0); + + for (unsigned int i = frame.renders.boundingBox.getTop(); i < frame.renders.boundingBox.getBottom(); i += 2) { + for (unsigned int j = frame.renders.boundingBox.getLeft(); j < frame.renders.boundingBox.getRight(); j += 2) { + double u = static_cast(j), v = static_cast(i); + double x = 0.0, y = 0.0; + double Z = frame.renders.depth[i][j]; + if (Z > 0.f) { + vpPixelMeterConversion::convertPoint(frame.cam, u, v, x, y); + cX[0] = x * Z; + cX[1] = y * Z; + cX[2] = Z; + oX = oMcprev * cX; + vpStored3DSurfaceColorPoint newPoint; + newPoint.X[0] = oX[0] / oX[3]; + newPoint.X[1] = oX[1] / oX[3]; + newPoint.X[2] = oX[2] / oX[3]; + const vpRGBa &c = previousFrame.IRGB[i][j]; + const float colorVariance = std::pow(static_cast(m_initialColorSigma), 2); + newPoint.stats.init(vpRGBf(c.R, c.G, c.B), vpRGBf(colorVariance)); + bool canAdd = true; + for (const vpStored3DSurfaceColorPoint &p : m_points) { + if (p.squaredDist(newPoint.X) < vpMath::sqr(m_minDist3DNewPoint)) { + canAdd = false; + break; + } + } + if (canAdd) { + m_points.push_back(newPoint); + } + } + } + } +} + +void vpRBProbabilistic3DDriftDetector::display(const vpImage &/*I*/) +{ + // for (const vpStored3DSurfaceColorPoint &p : m_points) { + // if (p.visible) { + // const vpRGBf color = p.stats.mean; + // // vpDisplay::displayPoint(I, p.projCurrPx[1], p.projCurrPx[0], vpColor::blue); + // vpDisplay::displayPoint(I, p.projCurrPx[1], p.projCurrPx[0], vpColor(color.R, color.G, color.B), 3); + // // vpDisplay::displayPoint(I, p.projCurrPx[1], p.projCurrPx[0], vpColor::white, 3); + + // // vpDisplay::displayLine(I, p.projCurrPx[1], p.projCurrPx[0], p.projPrevPx[1], p.projPrevPx[0], vpColor::red); + // } + // } +} + +double vpRBProbabilistic3DDriftDetector::getScore() const +{ + return m_score; +} + +bool vpRBProbabilistic3DDriftDetector::hasDiverged() const +{ + return m_score < 0.2; +} + +#if defined(VISP_HAVE_NLOHMANN_JSON) +void vpRBProbabilistic3DDriftDetector::loadJsonConfiguration(const nlohmann::json &j) +{ + setColorUpdateRate(j.value("colorUpdateRate", m_colorUpdateRate)); + setDepthStandardDeviation(j.value("depthSigma", m_depthSigma)); + setFilteringMax3DError(j.value("filteringMaxDistance", m_maxError3D)); + setInitialColorStandardDeviation(j.value("initialColorSigma", m_initialColorSigma)); + setMinDistForNew3DPoints(j.value("minDistanceNewPoints", m_minDist3DNewPoint)); +} +#endif + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp b/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp new file mode 100644 index 0000000000..6b3ec0e50f --- /dev/null +++ b/modules/tracker/rbt/src/features/vpRBDenseDepthTracker.cpp @@ -0,0 +1,165 @@ +/* + * 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 +#include +BEGIN_VISP_NAMESPACE + +void fastRotationMatmul(const vpRotationMatrix &cRo, const vpRGBf &v, vpColVector &res) +{ + res.resize(3, false); + const double r = static_cast(v.R), g = static_cast(v.G), b = static_cast(v.B); + const double *R = cRo.data; + res[0] = R[0] * r + R[1] * g + R[2] * b; + res[1] = R[3] * r + R[4] * g + R[5] * b; + res[2] = R[6] * r + R[7] * g + R[8] * b; +} + +void fastProjection(const vpHomogeneousMatrix &oTc, double X, double Y, double Z, vpPoint &p) +{ + const double *T = oTc.data; + p.set_oX(T[0] * X + T[1] * Y + T[2] * Z + T[3]); + p.set_oY(T[4] * X + T[5] * Y + T[6] * Z + T[7]); + p.set_oZ(T[8] * X + T[9] * Y + T[10] * Z + T[11]); + p.set_oW(1.0); +} + +void vpRBDenseDepthTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &/*previousFrame*/, const vpHomogeneousMatrix &/*cMo*/) +{ + const vpImage &depthMap = frame.depth; + const vpImage &renderDepth = frame.renders.depth; + vpRect bb = frame.renders.boundingBox; + const vpHomogeneousMatrix &cMo = frame.renders.cMo; + vpHomogeneousMatrix oMc = cMo.inverse(); + vpRotationMatrix cRo = cMo.getRotationMatrix(); + bool useMask = m_useMask && frame.hasMask(); + m_depthPoints.clear(); + m_depthPoints.reserve(static_cast(bb.getArea() / (m_step * m_step * 2))); + vpDepthPoint point; + for (unsigned int i = static_cast(bb.getTop()); i < static_cast(bb.getBottom()); i += m_step) { + for (unsigned int j = static_cast(bb.getLeft()); j < static_cast(bb.getRight()); j += m_step) { + // if (renderDepth[i][j] > frame.renders.zNear && renderDepth[i][j] < frame.renders.zFar && depthMap[i][j] > frame.renders.zNear * 0.33 && depthMap[i][j] < frame.renders.zFar * 3.0) { + double Z = renderDepth[i][j]; + double currZ = depthMap[i][j]; + if (Z > 0.f && currZ > 0.f) { + if (useMask && frame.mask[i][j] < m_minMaskConfidence) { + continue; + } + double x = 0.0, y = 0.0; + vpPixelMeterConversion::convertPoint(frame.cam, j, i, x, y); + //vpColVector objectNormal({ frame.renders.normals[i][j].R, frame.renders.normals[i][j].G, frame.renders.normals[i][j].B }); + point.objectNormal[0] = frame.renders.normals[i][j].R; + point.objectNormal[1] = frame.renders.normals[i][j].G; + point.objectNormal[2] = frame.renders.normals[i][j].B; + + //fastRotationMatmul(cRo, frame.renders.normals[i][j], point.objectNormal); + //vpColVector cameraNormal = cRo * objectNormal; + // if (acos(cameraNormal * vpColVector({ 0.0, 0.0, -1.0 })) > vpMath::rad(70.0)) { + // continue; + // } + // vpColVector cp({ x * Z, y * Z, Z, 1 }); + // vpColVector oP = oMc * cp; + fastProjection(oMc, x * Z, y * Z, Z, point.oP); + // point.oP = vpPoint(oP); + point.pixelPos.set_ij(i, j); + point.currentPoint[0] = x * currZ; + point.currentPoint[1] = y * currZ; + point.currentPoint[2] = currZ; + + m_depthPoints.push_back(point); + } + } + } + if (m_depthPoints.size() > 0) { + m_error.resize(m_depthPoints.size(), false); + m_weights.resize(m_depthPoints.size(), false); + m_weighted_error.resize(m_depthPoints.size(), false); + m_L.resize(m_depthPoints.size(), 6, false, false); + m_numFeatures = m_L.getRows(); + m_cov.resize(6, 6, false, false); + m_covWeightDiag.resize(m_depthPoints.size(), false); + } + else { + m_numFeatures = 0; + } +} + +void vpRBDenseDepthTracker::computeVVSIter(const vpRBFeatureTrackerInput &/*frame*/, const vpHomogeneousMatrix &cMo, unsigned int /*iteration*/) +{ + if (m_numFeatures == 0) { + m_LTL = 0; + m_LTR = 0; + m_error = 0; + m_weights = 1.0; + m_weighted_error = 0.0; + m_cov = 0.0; + m_covWeightDiag = 0.0; + return; + } + vpRotationMatrix cRo = cMo.getRotationMatrix(); +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel for +#endif + for (unsigned int i = 0; i < m_depthPoints.size(); ++i) { + vpDepthPoint &depthPoint = m_depthPoints[i]; + depthPoint.update(cMo, cRo); + depthPoint.error(m_error, i); + depthPoint.interaction(m_L, i); + } + + //m_weights = 0.0; + //m_robust.setMinMedianAbsoluteDeviation(1e-3); + m_robust.MEstimator(vpRobust::TUKEY, m_error, m_weights); + for (unsigned int i = 0; i < m_depthPoints.size(); ++i) { + m_weighted_error[i] = m_error[i] * m_weights[i]; + m_covWeightDiag[i] = m_weights[i] * m_weights[i]; + for (unsigned int dof = 0; dof < 6; ++dof) { + m_L[i][dof] *= m_weights[i]; + } + } + + m_LTL = m_L.AtA(); + computeJTR(m_L, m_weighted_error, m_LTR); + m_vvsConverged = false; +} + +void vpRBDenseDepthTracker::display(const vpCameraParameters &/*cam*/, const vpImage &/*I*/, + const vpImage &/*IRGB*/, const vpImage &depth) const +{ + for (unsigned int i = 0; i < m_depthPoints.size(); ++i) { + const vpDepthPoint &p = m_depthPoints[i]; + vpColor c(0, static_cast(m_weights[i] * 255), 0); + vpDisplay::displayPoint(depth, p.pixelPos, c, 2); + } + +} + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/features/vpRBFeatureTracker.cpp b/modules/tracker/rbt/src/features/vpRBFeatureTracker.cpp new file mode 100644 index 0000000000..ed73e4d3f1 --- /dev/null +++ b/modules/tracker/rbt/src/features/vpRBFeatureTracker.cpp @@ -0,0 +1,83 @@ +/* + * 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_SIMDLIB) +#include +#endif + +BEGIN_VISP_NAMESPACE + +vpRBFeatureTracker::vpRBFeatureTracker() +{ + m_numFeatures = 0; + m_userVvsWeight = 1.0; + m_vvsConverged = false; + m_enableDisplay = true; +} + +void vpRBFeatureTracker::updateCovariance(const double lambda) +{ + vpMatrix D; + D.diag(m_covWeightDiag); + m_cov = computeCovarianceMatrix(m_L, lambda * m_error, D); +} + +void vpRBFeatureTracker::computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR) +{ + if (interaction.getRows() != error.getRows() || interaction.getCols() != 6) { + throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "Incorrect matrices size in computeJTR."); + } + + JTR.resize(6, false); +#if defined(VISP_HAVE_SIMDLIB) + SimdComputeJtR(interaction.data, interaction.getRows(), error.data, JTR.data); +#else + const unsigned int N = interaction.getRows(); + + for (unsigned int i = 0; i < 6; ++i) { + double ssum = 0; + for (unsigned int j = 0; j < N; ++j) { + ssum += interaction[j][i] * error[j]; + } + JTR[i] = ssum; +} +#endif +} + +vpMatrix vpRBFeatureTracker::computeCovarianceMatrix(const vpMatrix &DJ, const vpColVector &e, const vpMatrix &covDiag) +{ + const vpColVector covDiagE = covDiag * e; + double sigma2 = (covDiagE.t() * covDiag * e) / ((double)e.getRows()); + return (DJ.t() * covDiag * DJ).pseudoInverse() * sigma2; +} + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/features/vpRBFeatureTrackerFactory.cpp b/modules/tracker/rbt/src/features/vpRBFeatureTrackerFactory.cpp new file mode 100644 index 0000000000..7729bd8028 --- /dev/null +++ b/modules/tracker/rbt/src/features/vpRBFeatureTrackerFactory.cpp @@ -0,0 +1,70 @@ +/* + * 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 +#include +#include +#include + +BEGIN_VISP_NAMESPACE + +vpRBFeatureTrackerFactory::vpRBFeatureTrackerFactory() +{ + setJsonKeyFinder([](const nlohmann::json &j) -> std::string { + return j.at("type"); + }); + + registerType("silhouetteMe", [](const nlohmann::json &j) { + std::shared_ptr p(new vpRBSilhouetteMeTracker()); + p->loadJsonConfiguration(j); + return p; + }); + registerType("silhouetteColor", [](const nlohmann::json &j) { + std::shared_ptr p(new vpRBSilhouetteCCDTracker()); + p->loadJsonConfiguration(j); + return p; + }); + registerType("depth", [](const nlohmann::json &j) { + std::shared_ptr p(new vpRBDenseDepthTracker()); + p->loadJsonConfiguration(j); + return p; + }); +#if defined(VP_HAVE_RB_KLT_TRACKER) + registerType("klt", [](const nlohmann::json &j) { + std::shared_ptr p(new vpRBKltTracker()); + p->loadJsonConfiguration(j); + return p; + }); +#endif +} + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/features/vpRBKltTracker.cpp b/modules/tracker/rbt/src/features/vpRBKltTracker.cpp new file mode 100644 index 0000000000..91f284a7bc --- /dev/null +++ b/modules/tracker/rbt/src/features/vpRBKltTracker.cpp @@ -0,0 +1,325 @@ +/* + * 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(VP_HAVE_RB_KLT_TRACKER) + +#include +#include +#include +#include + +BEGIN_VISP_NAMESPACE + +inline bool isTooCloseToBorder(unsigned int i, unsigned int j, unsigned int h, unsigned w, unsigned int border) +{ + return i < border || j < border || i >(h - border) || j >(w - border); +} + +inline void vpRBKltTracker::tryAddNewPoint( + const vpRBFeatureTrackerInput &frame, + std::map &points, + long id, const float u, const float v, + const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc) +{ + unsigned int uu = static_cast(round(u)), uv = static_cast(round(v)); + if (isTooCloseToBorder(uv, uu, frame.renders.depth.getRows(), frame.renders.depth.getCols(), m_border)) { + return; + } + + float Z = frame.renders.depth[uv][uu]; + if (Z <= 0.f || (frame.hasDepth() && frame.depth[uv][uu] > 0.f && fabs(frame.depth[uv][uu] - Z) > 5e-3)) { + return; + } + vpRBKltTracker::vpTrackedKltPoint p; + p.cTo0 = cMo; + vpRGBf normalRGB = frame.renders.normals[uv][uu]; + p.normal = vpColVector({ normalRGB.R, normalRGB.G, normalRGB.B }); + double x = 0.0, y = 0.0; + vpPixelMeterConversion::convertPoint(frame.cam, static_cast(u), static_cast(v), x, y); + vpColVector oC({ x * Z, y * Z, Z, 1.0 }); + vpColVector oX = oMc * oC; + oX /= oX[3]; + p.oX = vpPoint(oX[0], oX[1], oX[2]); + p.currentPos = vpImagePoint(y, x); + points[id] = p; + +} + +vpRBKltTracker::vpRBKltTracker() : + vpRBFeatureTracker(), m_numPointsReinit(20), m_newPointsDistanceThreshold(5.0), m_border(5), + m_maxErrorOutliersPixels(10.0), m_useMask(false), m_minMaskConfidence(0.0) +{ } + +void vpRBKltTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput & /*previousFrame*/, const vpHomogeneousMatrix &/*cMo*/) +{ + m_Iprev = m_I; + vpImageConvert::convert(frame.I, m_I); + const vpHomogeneousMatrix &cMo = frame.renders.cMo; + const vpHomogeneousMatrix oMc = cMo.inverse(); + if (m_maxErrorOutliersPixels > 0.0) { + const double distanceThresholdPxSquare = vpMath::sqr(m_maxErrorOutliersPixels); + const unsigned int nbFeatures = static_cast(m_klt.getNbFeatures()); + std::vector kltIndicesToRemove; + // Detect outliers + for (unsigned int i = 0; i < nbFeatures; ++i) { + long id = 0; + float u = 0.f, v = 0.f; + m_klt.getFeature(i, id, u, v); + if (m_points.find(id) == m_points.end()) { + continue; + } + unsigned int uu = static_cast(round(u)), uv = static_cast(round(v)); + if (isTooCloseToBorder(uv, uu, frame.renders.depth.getRows(), frame.renders.depth.getCols(), m_border)) { + continue; + } + const float Z = frame.renders.depth[uv][uu]; + if (Z <= 0.f) { + continue; + } + + vpTrackedKltPoint &p = m_points[id]; + double x = 0.0, y = 0.0; + vpPixelMeterConversion::convertPoint(frame.cam, static_cast(u), static_cast(v), x, y); + vpColVector oXn = oMc * vpColVector({ x * Z, y * Z, Z, 1.0 }); + oXn /= oXn[3]; + p.update(cMo); + double x1 = p.oX.get_x(), y1 = p.oX.get_y(); + double u1 = 0.0, v1 = 0.0; + vpMeterPixelConversion::convertPoint(frame.cam, x1, y1, u1, v1); + double distancePx = vpMath::sqr(u1 - u) + vpMath::sqr(v1 - v); + + vpColVector oX = p.oX.get_oP(); + if (distancePx > distanceThresholdPxSquare) { + m_points.erase(id); + kltIndicesToRemove.push_back(i); + } + } + // Remove tracking from klt: iterate in reverse order to invalidate iterator i (shifting in the klt list) + for (int i = static_cast(kltIndicesToRemove.size()) - 1; i >= 0; --i) { + m_klt.suppressFeature(kltIndicesToRemove[i]); + } + } + + cv::Mat mask = cv::Mat::zeros(m_I.rows, m_I.cols, CV_8U); + const vpRect bb = frame.renders.boundingBox; + for (unsigned int i = static_cast(bb.getTop()); i < static_cast(bb.getBottom()); ++i) { + for (unsigned int j = static_cast(bb.getLeft()); j < static_cast(bb.getRight()); ++j) { + mask.at(i, j) = (frame.renders.depth[i][j] > 0.f) * 255; + } + } + + cv::Rect roi(bb.getLeft(), bb.getTop(), bb.getWidth(), bb.getHeight()); + cv::Mat maskRoi = mask(roi); + + if (m_Iprev.rows > 0) { + // Consider that there are not enough points: reinit KLT tracking + if (m_points.size() < m_numPointsReinit) { + cv::Mat IprevRoi = m_Iprev(roi); + m_klt.initTracking(m_Iprev, mask); + const unsigned int nbFeatures = static_cast(m_klt.getNbFeatures()); + m_points.clear(); + for (unsigned int i = 0; i < nbFeatures; ++i) { + long id; + float u, v; + m_klt.getFeature(i, id, u, v); + tryAddNewPoint(frame, m_points, id, u, v, cMo, oMc); + } + } + else { // Otherwise, try and get new points + vpKltOpencv kltTemp; + kltTemp.setMaxFeatures(m_klt.getMaxFeatures()); + kltTemp.setWindowSize(m_klt.getWindowSize()); + kltTemp.setQuality(m_klt.getQuality()); + kltTemp.setMinDistance(m_klt.getMinDistance()); + kltTemp.setHarrisFreeParameter(m_klt.getHarrisFreeParameter()); + kltTemp.setBlockSize(m_klt.getBlockSize()); + kltTemp.setPyramidLevels(m_klt.getPyramidLevels()); + kltTemp.initTracking(m_Iprev(roi), maskRoi); + const unsigned int nbFeaturesTemp = static_cast(kltTemp.getNbFeatures()); + const unsigned int nbFeatures = static_cast(m_klt.getNbFeatures()); + for (unsigned int i = 0; i < nbFeaturesTemp; ++i) { + double threshold = vpMath::sqr(m_newPointsDistanceThreshold); // distance threshold, in squared pixels + double tooClose = false; + float u, v; + long id; + kltTemp.getFeature(i, id, u, v); + // Realign features from bounding box coordinates to image coordinates + u += bb.getLeft(); + v += bb.getTop(); + for (unsigned int j = 0; j < nbFeatures; ++j) { + float uj, vj; + long idj; + m_klt.getFeature(j, idj, uj, vj); + if (vpMath::sqr(uj - u) + vpMath::sqr(vj - v) < threshold) { + tooClose = true; + break; + } + } + if (tooClose) { + continue; + } + + m_klt.addFeature(u, v); + const std::vector &ids = m_klt.getFeaturesId(); + id = ids[ids.size() - 1]; + tryAddNewPoint(frame, m_points, id, u, v, cMo, oMc); + } + } + } + else { + m_klt.initTracking(m_I, mask); + m_points.clear(); + const unsigned int nbFeatures = static_cast(m_klt.getNbFeatures()); + for (unsigned int i = 0; i < nbFeatures; ++i) { + long id; + float u, v; + m_klt.getFeature(i, id, u, v); + tryAddNewPoint(frame, m_points, id, u, v, cMo, oMc); + } + } +} + +void vpRBKltTracker::trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &/*previousFrame*/, const vpHomogeneousMatrix &cMo) +{ + unsigned int nbKltFeatures = static_cast(m_klt.getNbFeatures()); + if (nbKltFeatures > 0) { + m_klt.track(m_I); + } + std::map newPoints; + const vpHomogeneousMatrix oMc = cMo.inverse(); + + bool testMask = m_useMask && frame.hasMask(); + nbKltFeatures = static_cast(m_klt.getNbFeatures()); + std::vector kltIndicesToRemove; + for (unsigned int i = 0; i < nbKltFeatures; ++i) { + long id = 0; + float u = 0.f, v = 0.f; + double x = 0.0, y = 0.0; + m_klt.getFeature(i, id, u, v); + unsigned int uu = static_cast(round(u)), uv = static_cast(round(v)); + // Filter points that are too close to image borders and cannot be reliably tracked + if (isTooCloseToBorder(uv, uu, frame.renders.depth.getRows(), frame.renders.depth.getCols(), m_border)) { + continue; + } + float Z = frame.renders.depth[uv][uu]; + if (Z <= 0.f) { + continue; + } + + if (testMask && frame.mask[uv][uu] < m_minMaskConfidence) { + continue; + } + + if (m_points.find(id) != m_points.end()) { + vpTrackedKltPoint &p = m_points[id]; + if (p.rotationDifferenceToInitial(oMc) > vpMath::rad(45.0) && p.normalDotProd(cMo) < cos(vpMath::rad(70))) { + continue; + } + vpPixelMeterConversion::convertPoint(frame.cam, static_cast(u), static_cast(v), x, y); + p.currentPos = vpImagePoint(y, x); + newPoints[id] = p; + } + else { + kltIndicesToRemove.push_back(i); + } + } + + // Remove tracking from klt: iterate in reverse order to invalidate iterator i (shifting in the klt list) + for (int i = static_cast(kltIndicesToRemove.size()) - 1; i >= 0; --i) { + m_klt.suppressFeature(kltIndicesToRemove[i]); + } + + m_points = newPoints; + m_numFeatures = m_points.size() * 2; +} + +void vpRBKltTracker::initVVS(const vpRBFeatureTrackerInput &/*frame*/, const vpRBFeatureTrackerInput &/*previousFrame*/, const vpHomogeneousMatrix & /*cMo*/) +{ + if (m_numFeatures < m_numPointsReinit * 2) { + m_numFeatures = 0; + return; + } + m_L.resize(m_numFeatures, 6, false, false); + m_error.resize(m_numFeatures, false); + m_weighted_error.resize(m_numFeatures, false); + m_weights.resize(m_numFeatures, false); + m_LTL.resize(6, 6, false, false); + m_LTR.resize(6, false); + m_cov.resize(6, 6, false, false); + m_covWeightDiag.resize(m_numFeatures, false); + m_error = 0; +} + +void vpRBKltTracker::computeVVSIter(const vpRBFeatureTrackerInput &/*frame*/, const vpHomogeneousMatrix &cMo, unsigned int /*iteration*/) +{ + if (m_numFeatures < m_numPointsReinit * 2) { + m_LTL = 0; + m_LTR = 0; + m_error = 0; + return; + } + unsigned int pointIndex = 0; + + for (std::pair &p : m_points) { + p.second.update(cMo); + p.second.interaction(m_L, pointIndex); + p.second.error(m_error, pointIndex); + ++pointIndex; + } + + //m_robust.setMinMedianAbsoluteDeviation(2.0 / frame.cam.get_px()); + m_robust.MEstimator(vpRobust::TUKEY, m_error, m_weights); + for (unsigned int i = 0; i < m_error.getRows(); ++i) { + m_weighted_error[i] = m_error[i] * m_weights[i]; + m_covWeightDiag[i] = m_weights[i] * m_weights[i]; + for (unsigned int dof = 0; dof < 6; ++dof) { + m_L[i][dof] *= m_weights[i]; + } + } + + m_LTL = m_L.AtA(); + computeJTR(m_L, m_weighted_error, m_LTR); +} + +void vpRBKltTracker::display(const vpCameraParameters &cam, const vpImage &I, + const vpImage &/*IRGB*/, const vpImage &/*depth*/) const +{ + for (const std::pair &p : m_points) { + double u = 0.0, v = 0.0; + vpMeterPixelConversion::convertPoint(cam, p.second.currentPos.get_j(), p.second.currentPos.get_i(), u, v); + vpDisplay::displayPoint(I, v, u, vpColor::red, 2); + } +} + +#endif + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp new file mode 100644 index 0000000000..c9b5e9166e --- /dev/null +++ b/modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp @@ -0,0 +1,751 @@ + +/* + * 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 + +#ifdef VISP_HAVE_OPENMP +#include +#endif + +#define VISP_DEBUG_CCD_TRACKER 0 + +BEGIN_VISP_NAMESPACE + +template class FastMat33 +{ +public: + std::array data; + + FastMat33() { } + + inline T operator[](const size_t i) const { return data[i]; } + + inline T &operator[](const size_t i) { return data[i]; } + + void inverse(FastMat33 &minv) const + { + // determinant + T det = data[0] * (data[4] * data[8] - data[7] * data[5]) - data[1] * (data[3] * data[8] - data[5] * data[6]) + + data[2] * (data[3] * data[7] - data[4] * data[6]); + T invdet = 1 / det; + + minv[0] = (data[4] * data[8] - data[7] * data[5]) * invdet; + minv[1] = (data[2] * data[7] - data[1] * data[8]) * invdet; + minv[2] = (data[1] * data[5] - data[2] * data[4]) * invdet; + minv[3] = (data[5] * data[6] - data[3] * data[8]) * invdet; + minv[4] = (data[0] * data[8] - data[2] * data[6]) * invdet; + minv[5] = (data[3] * data[2] - data[0] * data[5]) * invdet; + minv[6] = (data[3] * data[7] - data[6] * data[4]) * invdet; + minv[7] = (data[6] * data[1] - data[0] * data[7]) * invdet; + minv[8] = (data[0] * data[4] - data[3] * data[1]) * invdet; + } + + static void multiply(const vpMatrix &A, const FastMat33 &B, vpMatrix &C) + { + C.resize(A.getRows(), 3, false, false); + for (unsigned int i = 0; i < A.getRows(); ++i) { + C[i][0] = A[i][0] * B.data[0] + A[i][1] * B.data[3] + A[i][2] * B.data[6]; + C[i][1] = A[i][0] * B.data[1] + A[i][1] * B.data[4] + A[i][2] * B.data[7]; + C[i][2] = A[i][0] * B.data[2] + A[i][1] * B.data[5] + A[i][2] * B.data[8]; + } + } +}; + +template class FastMat63 +{ +public: + std::array data; + + FastMat63() { } + + inline T operator[](const size_t i) const { return data[i]; } + + inline T &operator[](const size_t i) { return data[i]; } + + static void multiply(const FastMat63 &A, const FastMat33 &B, FastMat63 &C) + { + + for (unsigned int i = 0; i < 6; ++i) { + const T *d = &A.data[i * 3]; + T *c = &C.data[i * 3]; + c[0] = d[0] * B.data[0] + d[1] * B.data[3] + d[2] * B.data[6]; + c[1] = d[0] * B.data[1] + d[1] * B.data[4] + d[2] * B.data[7]; + c[2] = d[0] * B.data[2] + d[1] * B.data[5] + d[2] * B.data[8]; + } + } + + static void multiplyBTranspose(const FastMat63 &A, const FastMat63 &B, vpMatrix &C) + { + C.resize(6, 6, false, false); + for (unsigned int i = 0; i < 6; ++i) { + const double *a = &A.data[i * 3]; + double *c = C[i]; + + c[0] = a[0] * B[0] + a[1] * B[1] + a[2] * B[2]; + c[1] = a[0] * B[3] + a[1] * B[4] + a[2] * B[5]; + c[2] = a[0] * B[6] + a[1] * B[7] + a[2] * B[8]; + + c[3] = a[0] * B[9] + a[1] * B[10] + a[2] * B[11]; + c[4] = a[0] * B[12] + a[1] * B[13] + a[2] * B[14]; + c[5] = a[0] * B[15] + a[1] * B[16] + a[2] * B[17]; + } + } +}; + +template class FastVec3 +{ +public: + std::array data; + + inline T operator[](const size_t i) const { return data[i]; } + inline T &operator[](const size_t i) { return data[i]; } + + static void multiply(const FastMat63 &A, const FastVec3 &B, vpColVector &C) + { + C[0] = A[0] * B[0] + A[1] * B[1] + A[2] * B[2]; + C[1] = A[3] * B[0] + A[4] * B[1] + A[5] * B[2]; + C[2] = A[6] * B[0] + A[7] * B[1] + A[8] * B[2]; + C[3] = A[9] * B[0] + A[10] * B[1] + A[11] * B[2]; + C[4] = A[12] * B[0] + A[13] * B[1] + A[14] * B[2]; + C[5] = A[15] * B[0] + A[16] * B[1] + A[17] * B[2]; + } + +}; + +vpRBSilhouetteCCDTracker::vpRBSilhouetteCCDTracker() : vpRBFeatureTracker(), m_vvsConvergenceThreshold(0.0), m_temporalSmoothingFac(0.1), m_useMask(false), m_minMaskConfidence(0.0), m_displayType(vpRBSilhouetteCCDDisplayType::SIMPLE) +{ } + +void vpRBSilhouetteCCDTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput & /*previousFrame*/, const vpHomogeneousMatrix &/*cMo*/) +{ + m_controlPoints.clear(); + //m_controlPoints.reserve(frame.silhouettePoints.size()); + const vpHomogeneousMatrix cMo = frame.renders.cMo; + const vpHomogeneousMatrix oMc = cMo.inverse(); + for (const vpRBSilhouettePoint &sp : frame.silhouettePoints) { + // std::cout << m_ccdParameters.h << std::endl; + // std::cout << sp.j << ", " << sp.i << std::endl; + int ii = sp.i, jj = sp.j; + + if (ii <= m_ccdParameters.h || jj <= m_ccdParameters.h || static_cast(ii) >= frame.I.getHeight() - m_ccdParameters.h || static_cast(jj) >= frame.I.getWidth() - m_ccdParameters.h) { + continue; + } + vpRBSilhouetteControlPoint pccd; + pccd.buildSilhouettePoint(ii, jj, sp.Z, sp.orientation, sp.normal, cMo, oMc, frame.cam); + + pccd.detectSilhouette(frame.renders.depth); + if (!pccd.isSilhouette() || std::isnan(sp.orientation) || !pccd.isValid()) { + continue; + } + + if (frame.hasMask() && m_useMask) { + double maskGradValue = computeMaskGradient(frame.mask, pccd); + if (maskGradValue < m_minMaskConfidence) { + continue; + } + } + m_controlPoints.push_back(std::move(pccd)); + + } +} + +double vpRBSilhouetteCCDTracker::computeMaskGradient(const vpImage &mask, const vpRBSilhouetteControlPoint &pccd) const +{ + + std::vector maskValues(m_ccdParameters.h * 2 + 1); + double c = cos(pccd.getTheta()); + double s = sin(pccd.getTheta()); + int index = 0; + for (int n = -m_ccdParameters.h + 1; n < m_ccdParameters.h; ++n) { + unsigned int ii = static_cast(round(pccd.icpoint.get_i() + s * n)); + unsigned int jj = static_cast(round(pccd.icpoint.get_j() + c * n)); + maskValues[index] = mask[ii][jj]; + ++index; + } + + double maxGrad = 0.0; + + for (unsigned i = 1; i < maskValues.size() - 1; ++i) { + double grad = abs(maskValues[i + 1] - maskValues[i - 1]); + if (grad > maxGrad) { + maxGrad = grad; + } + } + return maxGrad; +} + +void vpRBSilhouetteCCDTracker::initVVS(const vpRBFeatureTrackerInput &/*frame*/, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix & /*cMo*/) +{ + // Reinit all variables + m_sigma = vpMatrix(m_ccdParameters.phi_dim, m_ccdParameters.phi_dim, 0.0); + m_cov.resize(6, 6); + tol = 0.0; + m_vvsConverged = false; + + unsigned int resolution = m_controlPoints.size(); + int normal_points_number = floor(m_ccdParameters.h / m_ccdParameters.delta_h); + unsigned nerror_ccd = 2 * normal_points_number * 3 * resolution; + m_numFeatures = nerror_ccd; + + m_stats.reinit(resolution, normal_points_number); + m_prevStats.reinit(resolution, normal_points_number); + m_gradient = vpMatrix(m_ccdParameters.phi_dim, 1, 0.0); + m_hessian = vpMatrix(m_ccdParameters.phi_dim, m_ccdParameters.phi_dim, 0.0); + m_gradients.resize(m_controlPoints.size() * 2 * normal_points_number, vpColVector(m_gradient.getRows())); + m_hessians.resize(m_controlPoints.size() * 2 * normal_points_number, vpMatrix(m_hessian.getRows(), m_hessian.getCols())); + + //m_weights.resize(nerror_ccd); + m_weights.resize(m_numFeatures, false); + // m_weights = 1; + // computeMask(frame.renders.color, m_stats); + // computeMask(frame.renders.color, m_prevStats); + // m_stats.weight = 1.0; + // m_prevStats.weight = 1.0; + computeLocalStatistics(previousFrame.IRGB, m_prevStats); + //computeLocalStatistics(image, m_stats); +} + +void vpRBSilhouetteCCDTracker::computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int iteration) +{ + vpColVector oldPoints(m_controlPoints.size() * 2); + for (unsigned int i = 0; i < m_controlPoints.size(); ++i) { + oldPoints[i * 2] = m_controlPoints[i].icpoint.get_u(); + oldPoints[i * 2 + 1] = m_controlPoints[i].icpoint.get_v(); + } + updateCCDPoints(cMo); + + tol = 0.0; + for (unsigned int i = 0; i < m_controlPoints.size(); ++i) { + tol += abs(oldPoints[i * 2] - m_controlPoints[i].icpoint.get_u()); + tol += abs(oldPoints[i * 2 + 1] - m_controlPoints[i].icpoint.get_v()); + } + tol /= m_controlPoints.size(); + computeLocalStatistics(frame.IRGB, m_stats); + computeErrorAndInteractionMatrix(); // Update interaction matrix, and gauss newton left and right side terms + + m_vvsConverged = false; + if (iteration > 0 && tol < m_vvsConvergenceThreshold) { + m_vvsConverged = true; + } +} + +void vpRBSilhouetteCCDTracker::display(const vpCameraParameters &/*cam*/, const vpImage &/*I*/, + const vpImage &IRGB, const vpImage &/*depth*/) const +{ + unsigned normal_points_number = floor(m_ccdParameters.h / m_ccdParameters.delta_h); + unsigned nerror_per_point = 2 * normal_points_number * 3; + if (m_displayType == vpRBSilhouetteCCDDisplayType::SIMPLE) { + + for (unsigned int i = 0; i < m_controlPoints.size(); ++i) { + const vpRBSilhouetteControlPoint &p = m_controlPoints[i]; + vpDisplay::displayCross(IRGB, p.icpoint.get_i(), p.icpoint.get_j(), 3, vpColor::green, 1); + vpImagePoint diff(m_stats.nv[i][1] * m_ccdParameters.h, m_stats.nv[i][0] * m_ccdParameters.h); + + // vpImagePoint ip2 = p.icpoint + diff; + // vpDisplay::displayArrow(IRGB, p.icpoint, ip2, p.invnormal ? vpColor::red : vpColor::lightBlue); + } + } + else if (m_displayType == vpRBSilhouetteCCDDisplayType::ERROR) { + vpColVector errorPerPoint(m_controlPoints.size()); + double maxPointError = 0.0; + for (unsigned int i = 0; i < m_controlPoints.size(); ++i) { + double sum = 0.0; + for (unsigned int j = 0; j < nerror_per_point; ++j) { + sum += m_error[i * nerror_per_point + j]; + } + if (sum > maxPointError) { + maxPointError = sum; + } + errorPerPoint[i] = sum; + } + const vpColor bestColor = vpColor::green; + const vpColor worstColor = vpColor::red; + unsigned idx = 0; + for (const vpRBSilhouetteControlPoint &p : m_controlPoints) { + const double weight = errorPerPoint[idx] / maxPointError; + const double diffR = (double)(worstColor.R) - (double)(bestColor.R); + const double diffG = (double)(worstColor.G) - (double)(bestColor.G); + const double diffB = (double)(worstColor.B) - (double)(bestColor.B); + + vpColor c; + c.R = (unsigned char)((double)(bestColor.R) + diffR * weight); + c.G = (unsigned char)((double)(bestColor.G) + diffG * weight); + c.B = (unsigned char)((double)(bestColor.B) + diffB * weight); + + vpDisplay::displayCross(IRGB, p.icpoint.get_i(), p.icpoint.get_j(), 3, c, 1); + ++idx; + } + } + else if (m_displayType == vpRBSilhouetteCCDDisplayType::WEIGHT) { + vpColVector weightPerPoint(m_controlPoints.size()); + for (unsigned int i = 0; i < m_controlPoints.size(); ++i) { + double sum = 0.0; + for (unsigned int j = 0; j < nerror_per_point; ++j) { + sum += m_weights[i * nerror_per_point + j]; + } + + weightPerPoint[i] = sum / nerror_per_point; + } + const vpColor bestColor = vpColor::green; + unsigned idx = 0; + for (const vpRBSilhouetteControlPoint &p : m_controlPoints) { + const double weight = weightPerPoint[idx]; + vpColor c; + c.R = 0; + c.G = (unsigned char)(255.f * weight); + c.B = 0; + + vpDisplay::displayCross(IRGB, p.icpoint.get_i(), p.icpoint.get_j(), 3, c, 1); + idx++; + } + } + else { + throw vpException(vpException::badValue, "Unknown display type"); + } +} + +void vpRBSilhouetteCCDTracker::updateCCDPoints(const vpHomogeneousMatrix &cMo) +{ +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel for +#endif + for (vpRBSilhouetteControlPoint &p : m_controlPoints) { + p.updateSilhouettePoint(cMo); + } +} + +void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage &I, vpCCDStatistics &stats) +{ + + const double minus_exp_gamma2 = exp(-m_ccdParameters.gamma_2); + + const double sigma = m_ccdParameters.h / (m_ccdParameters.alpha * m_ccdParameters.gamma_3); + // sigma_hat = gamma_3 * sigma + + // double sigma_hat = max(h/sqrt(2*gamma_2), gamma_4); + const double sigma_hat = m_ccdParameters.gamma_3 * sigma + m_ccdParameters.gamma_4; + unsigned int resolution = m_controlPoints.size(); + // to save the normalized parameters of vic[i,8] + // dimension: resolution x 2 + // the first column save the normalized coefficient outside the curve + // the second column store the one inside the curve + vpMatrix normalized_param = vpMatrix(resolution, 2, 0.0); + +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel for +#endif + for (unsigned int kk = 0; kk < m_controlPoints.size(); kk++) { + // temporary points used to store those points in the + // normal direction as well as negative normal direction + std::array pt1, pt2; + + // store the distance from a point in normal(negative norml) direction + // to the point on the curve + std::array dist1, dist2; + + vpRBSilhouetteControlPoint &p = m_controlPoints[kk]; + int ccdh = m_ccdParameters.h; + if (p.icpoint.get_i() <= ccdh || p.icpoint.get_i() > (I.getHeight() - ccdh) || p.icpoint.get_j() <= ccdh || p.icpoint.get_j() > (I.getWidth() - ccdh)) { + p.setValid(false); // invalidate points that are too close to image border + } + + if (!p.isValid()) { + continue; + } + double *nv_ptr = stats.nv[kk]; + nv_ptr[0] = p.nxs; + nv_ptr[1] = p.nys; +#if VISP_DEBUG_CCD_TRACKER + if (std::isnan(nv_ptr[0]) || std::isnan(nv_ptr[1])) { + throw vpException(vpException::fatalError, "x: %f, theta = %f", p.xs, p.getTheta()); + } +#endif + + int k = 0; + double alpha = 0.5; + double *vic_ptr = stats.vic[kk]; + for (int j = m_ccdParameters.delta_h; j <= m_ccdParameters.h; j += m_ccdParameters.delta_h, k++) { + /////////////////////////////////////////////////////////////////////////////////////////// + // calculate in the direction +n: (n_x, n_y) + ///////////////////////////////////////////////////////////////////////////////////////// + // x_{k,l} + pt1[0] = round(p.icpoint.get_u() + j * nv_ptr[0]); + // y_{k,l} + pt1[1] = round(p.icpoint.get_v() + j * nv_ptr[1]); + // distance between x_{k,l} and x_{k,0} in the normal direction + // appoximately it is l*h, l = {1,2,3,.....} + dist1[0] = (pt1[0] - p.icpoint.get_u()) * nv_ptr[0] + (pt1[1] - p.icpoint.get_v()) * nv_ptr[1]; + // distance between y_{k,l} and y_{k,0} along the curve + // it approximates 0 + dist1[1] = (pt1[0] - p.icpoint.get_u()) * nv_ptr[1] - (pt1[1] - p.icpoint.get_v()) * nv_ptr[0]; + vic_ptr[10 * k + 0] = pt1[1]; + vic_ptr[10 * k + 1] = pt1[0]; + vic_ptr[10 * k + 2] = dist1[0]; + vic_ptr[10 * k + 3] = dist1[1]; + //std::cout << tmp1 << std::endl; + + // fuzzy assignment a(d_{k,l}) = 1/2*(erf(d_{kl})/\sqrt(2)*sigma) + 1/2 + vic_ptr[10 * k + 4] = 0.5 * (erf((dist1[0]) / (sqrt(2) * sigma)) + 1.0); + //vic_ptr[10*k + 4] = logistic(dist1[0]/(sqrt(2)*sigma)); + //double wp1 = (a_{d,l} - gamm_1) /(1-gamma_1) + double wp1 = (vic_ptr[10 * k + 4] - m_ccdParameters.gamma_1) / (1 - m_ccdParameters.gamma_1); + + // wp1^4, why? if a_{d,l} \approx 0.5, do not count the point + vic_ptr[10 * k + 5] = wp1 * wp1 * wp1 * wp1; + + // wp1 = (1-a_{d,l} - gamm_1) /(1-gamma_1) + // double wp2 = (1-vic_ptr[10*k + 4] - gamma_1)/(1-gamma_1); + double wp2 = (1 - vic_ptr[10 * k + 4] - 0.25); + vic_ptr[10 * k + 6] = -64 * wp2 * wp2 * wp2 * wp2 + 0.25; + // W_p(d_p, simga_p) = c*max[0, exp(-d_p^2/2*sigma_p'^2) - exp(-gamma_2))] + vic_ptr[10 * k + 7] = std::max((exp(-0.5 * dist1[0] * dist1[0] / (sigma_hat * sigma_hat)) - minus_exp_gamma2), 0.0); + // W' = 0.5*exp(-|d_v= - d_p=|/alpha)/alpha + vic_ptr[10 * k + 8] = 0.5 * exp(-abs(dist1[1]) / alpha) / alpha; + // the derivative of col_5: 1/(sqrt(2*PI)*sigma)*exp{-d_{k,l}^2/(2*sigma*sigma)} + vic_ptr[10 * k + 9] = exp(-dist1[0] * dist1[0] / (2 * sigma * sigma)) / (sqrt(2.0 * M_PI) * sigma); + + + // calculate the normalization parameter c + normalized_param[kk][0] += vic_ptr[10 * k + 7]; + + /////////////////////////////////////////////////////////////////////////////////////////// + // calculate in the direction -n: (-n_x, -n_y) + ///////////////////////////////////////////////////////////////////////////////////////// + pt2[0] = round(p.icpoint.get_u() - j * nv_ptr[0]); + pt2[1] = round(p.icpoint.get_v() - j * nv_ptr[1]); + + // cv::circle(canvas_tmp, cv::Point2d(pt2[0], pt2[1]), 1, CV_RGB(255,0,0), 1);#ifdef DEBUG + + // start compute the size in the direction of -(n_x, n_y) + dist2[0] = (pt2[0] - p.icpoint.get_u()) * nv_ptr[0] + (pt2[1] - p.icpoint.get_v()) * nv_ptr[1]; + dist2[1] = (pt2[0] - p.icpoint.get_u()) * nv_ptr[1] - (pt2[1] - p.icpoint.get_v()) * nv_ptr[0]; + int negative_normal = k + (int)floor(m_ccdParameters.h / m_ccdParameters.delta_h); + vic_ptr[10 * negative_normal + 0] = pt2[1]; + vic_ptr[10 * negative_normal + 1] = pt2[0]; + vic_ptr[10 * negative_normal + 2] = dist2[0]; + vic_ptr[10 * negative_normal + 3] = dist2[1]; + //std::cout << " u " << p.icpoint.get_u() << " v " << p.icpoint.get_v() << " dist " << dist2[0] << " nx " << nv_ptr[0] << " ny "<< nv_ptr[1] << " theta " << p.get_theta() << std::endl; + vic_ptr[10 * negative_normal + 4] = 0.5 * (erf(dist2[0] / (sqrt(2) * sigma)) + 1); + //vic_ptr[10*negative_normal + 4] = logistic(dist2[0]/(sqrt(2)*sigma)); + // vic_ptr[10*negative_normal + 4] = 0.5; + wp1 = (vic_ptr[10 * negative_normal + 4] - 0.25); + vic_ptr[10 * negative_normal + 5] = -64 * wp1 * wp1 * wp1 * wp1 + 0.25; + wp2 = (1 - vic_ptr[10 * negative_normal + 4] - m_ccdParameters.gamma_1) / (1 - m_ccdParameters.gamma_1); + vic_ptr[10 * negative_normal + 6] = wp2 * wp2 * wp2 * wp2; + vic_ptr[10 * negative_normal + 7] = std::max((exp(-0.5 * dist2[0] * dist2[0] / (sigma_hat * sigma_hat)) - minus_exp_gamma2), 0.0); + vic_ptr[10 * negative_normal + 8] = 0.5 * exp(-abs(dist2[0]) / alpha) / alpha; + vic_ptr[10 * negative_normal + 9] = exp(-dist2[0] * dist2[0] / (2 * sigma * sigma)) / (sqrt(2 * CV_PI) * sigma); + normalized_param[kk][1] += vic_ptr[10 * negative_normal + 7]; + } + } + +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel for +#endif + for (unsigned int i = 0; i < resolution; ++i) { + if (!m_controlPoints[i].isValid()) { + continue; + } + + int k = 0; + // w1 = \sum wp_1, w2 = \sum wp_2 + double w1 = 0.0, w2 = 0.0; + + // store mean value near the curve + std::array m1 { 0.0, 0.0, 0.0 }, m2 { 0.0, 0.0, 0.0 }; + + // store the second mean value near the curve + std::array m1_o2 { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + std::array m2_o2 { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // compute local statistics + + // start search the points in the +n direction as well as -n direction + double wp1 = 0.0, wp2 = 0.0; + + double *vic_ptr = stats.vic[i]; + double *mean_vic_ptr = stats.mean_vic[i]; + double *cov_vic_ptr = stats.cov_vic[i]; + double *pix_ptr = stats.imgPoints[i]; + + for (int j = m_ccdParameters.delta_h; j <= m_ccdParameters.h; j += m_ccdParameters.delta_h, k++) { + wp1 = 0.0, wp2 = 0.0; + int negative_normal = k + (int)floor(m_ccdParameters.h / m_ccdParameters.delta_h); + const double *vic_k = vic_ptr + 10 * k; + + // wp1 = w(a_{k,l})*w(d_{k,l})*w(d) + wp1 = (vic_k[5] * vic_k[7] / normalized_param[i][0]); + + // wp2 = w(a_{k,l})*w(d_{k,l})*w(d) + wp2 = (vic_k[6] * vic_k[7] / normalized_param[i][1]); + //w1 = \sum{wp1} + w1 += wp1; + + //w2 = \sum{wp2} + w2 += wp2; + + // compute the mean value in the vicinity of a point + // m_{ks} = I{k}^{s} = \sum_{l} w_{kls}{I_{kl}} : s = 1 or 2 + const vpRGBa pixelRGBa = I(vic_k[0], vic_k[1]); + double *pixel = pix_ptr + k * 3; + pixel[0] = pixelRGBa.R; + pixel[1] = pixelRGBa.G; + pixel[2] = pixelRGBa.B; + + m1[0] += wp1 * pixel[0]; + m1[1] += wp1 * pixel[1]; + m1[2] += wp1 * pixel[2]; + + m2[0] += wp2 * pixel[0]; + m2[1] += wp2 * pixel[1]; + m2[2] += wp2 * pixel[2]; + + // compute second order local statistics + // m_{k,s} = \sum_{l} w_{kls} I_{kl}*I_{kl}^T + for (unsigned int m = 0; m < 3; ++m) { + for (unsigned int n = 0; n < 3; ++n) { + m1_o2[m * 3 + n] += wp1 * pixel[m] * pixel[n]; + m2_o2[m * 3 + n] += wp2 * pixel[m] * pixel[n]; + } + } + const double *vic_neg = vic_ptr + 10 * negative_normal; + const vpRGBa pixelNegRGBa = I(vic_neg[0], vic_neg[1]); + double *pixelNeg = pix_ptr + negative_normal * 3; + + pixelNeg[0] = pixelNegRGBa.R; + pixelNeg[1] = pixelNegRGBa.G; + pixelNeg[2] = pixelNegRGBa.B; + wp1 = (vic_neg[5] * vic_neg[7] / normalized_param[i][0]); + wp2 = (vic_neg[6] * vic_neg[7] / normalized_param[i][1]); + w1 += wp1; + w2 += wp2; + + m1[0] += wp1 * pixelNeg[0]; + m1[1] += wp1 * pixelNeg[1]; + m1[2] += wp1 * pixelNeg[2]; + + m2[0] += wp2 * pixelNeg[0]; + m2[1] += wp2 * pixelNeg[1]; + m2[2] += wp2 * pixelNeg[2]; + + for (unsigned int m = 0; m < 3; ++m) { + for (unsigned int n = 0; n < 3; ++n) { + m1_o2[m * 3 + n] += wp1 * pixelNeg[m] * pixelNeg[n]; + m2_o2[m * 3 + n] += wp2 * pixelNeg[m] * pixelNeg[n]; + } + } + } + mean_vic_ptr[0] = m1[0] / w1; + mean_vic_ptr[1] = m1[1] / w1; + mean_vic_ptr[2] = m1[2] / w1; + + mean_vic_ptr[3] = m2[0] / w2; + mean_vic_ptr[4] = m2[1] / w2; + mean_vic_ptr[5] = m2[2] / w2; + + for (unsigned int m = 0; m < 3; ++m) { + for (unsigned int n = 0; n < 3; ++n) { + cov_vic_ptr[m * 3 + n] = m1_o2[m * 3 + n] / w1 - m1[m] * m1[n] / (w1 * w1); + cov_vic_ptr[9 + m * 3 + n] = m2_o2[m * 3 + n] / w2 - m2[m] * m2[n] / (w2 * w2); + } + cov_vic_ptr[m * 3 + m] += m_ccdParameters.kappa; + cov_vic_ptr[9 + m * 3 + m] += m_ccdParameters.kappa; + } + } +} + +void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix() +{ + const int npointsccd = m_controlPoints.size(); + const int normal_points_number = floor(m_ccdParameters.h / m_ccdParameters.delta_h); + const int nerror_ccd = 2 * normal_points_number * 3 * npointsccd; + m_error.resize(nerror_ccd, false); + m_weighted_error.resize(nerror_ccd, false); + m_L.resize(nerror_ccd, 6, false, false); +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel +#endif + { + // vpMatrix tmp_cov(3, 3); + // vpMatrix tmp_cov_inv(3, 3); + FastMat33 tmp_cov, tmp_cov_inv; + FastMat63 tmp_jacobian; + FastMat63 tmp_jacobian_x_tmp_cov_inv; + FastVec3 tmp_pixel_diff; + double Lnvp[6]; + unsigned int normal_points_number = static_cast(floor(m_ccdParameters.h / m_ccdParameters.delta_h)); + +#ifdef VISP_HAVE_OPENMP +#pragma omp for +#endif + for (unsigned int kk = 0; kk < m_controlPoints.size(); kk++) { + const int i = kk; + const vpRBSilhouetteControlPoint &p = m_controlPoints[kk]; + + if (!p.isValid()) { + for (unsigned int j = 0; j < 2 * normal_points_number; ++j) { + for (unsigned int m = 0; m < 3; ++m) { + m_error[i * 2 * normal_points_number * 3 + j * 3 + m] = 0.0; + } + } + continue; + } + + const double *vic_ptr = m_stats.vic[i]; + const double *nv_ptr = m_stats.nv[i]; + const double *mean_vic_ptr = m_stats.mean_vic[i]; + const double *cov_vic_ptr = m_stats.cov_vic[i]; + const double *pix_ptr = m_stats.imgPoints[i]; + + const double *mean_vic_ptr_prev = m_prevStats.mean_vic[i]; + const double *cov_vic_ptr_prev = m_prevStats.cov_vic[i]; + const vpCameraParameters &cam = p.getCameraParameters(); + + Lnvp[0] = (-nv_ptr[0] / p.Zs); + Lnvp[1] = (-nv_ptr[1] / p.Zs); + Lnvp[2] = ((nv_ptr[0] * p.xs + nv_ptr[1] * p.ys) / p.Zs); + Lnvp[3] = (nv_ptr[0] * p.xs * p.ys + nv_ptr[1] * (1.0 + p.ys * p.ys)); + Lnvp[4] = (-nv_ptr[1] * p.xs * p.ys - nv_ptr[0] * (1.0 + p.xs * p.xs)); + Lnvp[5] = (nv_ptr[0] * p.ys - nv_ptr[1] * p.xs); + + + for (unsigned int j = 0; j < 2 * normal_points_number; ++j) { + const double *vic_j = vic_ptr + 10 * j; + const double *pix_j = pix_ptr + j * 3; + const double errf = vic_j[4]; + const double smooth2 = m_temporalSmoothingFac * m_temporalSmoothingFac; + double *error_ccd_j = m_error.data + i * 2 * normal_points_number * 3 + j * 3; + + for (unsigned n = 0; n < 9; ++n) { + //double *tmp_cov_ptr = tmp_cov[m]; + tmp_cov[n] = errf * cov_vic_ptr[n] + (1.0 - errf) * cov_vic_ptr[n + 9] + + smooth2 * (errf * cov_vic_ptr_prev[n] + (1.0 - errf) * cov_vic_ptr_prev[n + 9]); + } + + tmp_cov.inverse(tmp_cov_inv); + + //compute the difference between I_{kl} and \hat{I_{kl}} + for (int m = 0; m < 3; ++m) { + double err = (pix_j[m] - errf * mean_vic_ptr[m] - (1.0 - errf) * mean_vic_ptr[m + 3]) + + m_temporalSmoothingFac * (pix_j[m] - errf * mean_vic_ptr_prev[m] - (1.0 - errf) * mean_vic_ptr_prev[m + 3]); + //error_ccd[i*2*normal_points_number*3 + j*3 + m] = img(vic_ptr[10*j+0], vic_ptr[10*j+1])[m]- errf * mean_vic_ptr[m]- (1-errf)* mean_vic_ptr[m+3]; + tmp_pixel_diff[m] = err; + error_ccd_j[m] = err; + } + + //compute jacobian matrix + //memset(tmp_jacobian.data, 0, 3 * m_ccdParameters.phi_dim * sizeof(double)); + for (unsigned int n = 0; n < 3; ++n) { + const double f = -cam.get_px() * (vic_j[9] * (mean_vic_ptr[n] - mean_vic_ptr[n + 3])); + const double facPrev = -cam.get_px() * m_temporalSmoothingFac * (vic_j[9] * (mean_vic_ptr_prev[n] - mean_vic_ptr_prev[n + 3])); + for (unsigned int dof = 0; dof < 6; ++dof) { + tmp_jacobian.data[dof * 3 + n] = f * Lnvp[dof] + facPrev * Lnvp[dof]; + } + } + + FastMat63::multiply(tmp_jacobian, tmp_cov_inv, tmp_jacobian_x_tmp_cov_inv); + //vpMatrix::mult2Matrices(tmp_jacobian, tmp_cov_inv, tmp_jacobian_x_tmp_cov_inv); + FastVec3::multiply(tmp_jacobian_x_tmp_cov_inv, tmp_pixel_diff, m_gradients[i * 2 * normal_points_number + j]); + FastMat63::multiplyBTranspose(tmp_jacobian_x_tmp_cov_inv, tmp_jacobian, m_hessians[i * 2 * normal_points_number + j]); + // vpMatrix::mult2Matrices(tmp_jacobian_x_tmp_cov_inv, tmp_jacobian.t(), m_hessians[i * 2 * normal_points_number + j]); + } + } + } + + m_gradient = 0.0; + m_hessian = 0.0; + //m_robust.setMinMedianAbsoluteDeviation(1.0); + m_robust.MEstimator(vpRobust::vpRobustEstimatorType::TUKEY, m_error, m_weights); + + for (unsigned int i = 0; i < m_L.getRows(); ++i) { + m_weighted_error[i] = m_error[i] * m_weights[i]; + for (unsigned int j = 0; j < 6; ++j) { + m_L[i][j] *= m_weights[i]; + } + } + + // Store all the gradients and hessians and then sum them up after the parallel region. This ensures that computation is determinist + std::vector localGradients; + std::vector localHessians; +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel +#endif + { + vpColVector localGradient(m_gradient.getRows(), 0.0); + vpMatrix localHessian(m_hessian.getRows(), m_hessian.getCols(), 0.0); +#ifdef VISP_HAVE_OPENMP +#pragma omp single +#endif + { +#ifdef VISP_HAVE_OPENMP + unsigned int threads = omp_get_num_threads(); +#else + unsigned int threads = 1; +#endif + localGradients.resize(threads, localGradient); + localHessians.resize(threads, localHessian); + } +#ifdef VISP_HAVE_OPENMP +#pragma omp for schedule(static) +#endif + for (unsigned int i = 0; i < m_gradients.size(); ++i) { + m_gradients[i] *= m_weights[i]; + m_hessians[i] *= m_weights[i]; + localHessian += m_hessians[i]; + localGradient += m_gradients[i]; + } +#ifdef VISP_HAVE_OPENMP + unsigned int currentThread = omp_get_thread_num(); +#else + unsigned int currentThread = 0; +#endif + localGradients[currentThread] = localGradient; + localHessians[currentThread] = localHessian; + } + for (unsigned int i = 0; i < localGradients.size(); ++i) { + m_gradient += localGradients[i]; + m_hessian += localHessians[i]; + } + + m_LTL = m_hessian; + m_LTR = -m_gradient; + try { + vpMatrix hessian_E_inv = m_hessian.inverseByCholesky(); + //m_sigma = /*m_sigma +*/ 2*hessian_E_inv; + m_sigma = m_ccdParameters.covarianceIterDecreaseFactor * m_sigma + 2.0 * (1.0 - m_ccdParameters.covarianceIterDecreaseFactor) * hessian_E_inv; + } + catch (vpException &e) { + std::cerr << "Inversion issues in CCD tracker" << std::endl; + } + +} + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp b/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp new file mode 100644 index 0000000000..f9e058acf1 --- /dev/null +++ b/modules/tracker/rbt/src/features/vpRBSilhouetteMeTracker.cpp @@ -0,0 +1,197 @@ +/* + * 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 + +#define VISP_DEBUG_ME_TRACKER 0 + +BEGIN_VISP_NAMESPACE + +/** + * @brief Extract the geometric features from the list of collected silhouette points +*/ +void vpRBSilhouetteMeTracker::extractFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &previousFrame, const vpHomogeneousMatrix &/*cMo*/) +{ + m_controlPoints.clear(); + m_controlPoints.reserve(frame.silhouettePoints.size()); + const vpHomogeneousMatrix &cMo = frame.renders.cMo; + const vpHomogeneousMatrix oMc = cMo.inverse(); + vpColVector oC = oMc.getRotationMatrix() * vpColVector({ 0.0, 0.0, -1.0 }); + for (const vpRBSilhouettePoint &sp: frame.silhouettePoints) { + // float angle = vpMath::deg(acos(sp.normal * oC)); + // if (angle > 89.0) { + // continue; + // } + // std::cout << angle << std::endl; +#if VISP_DEBUG_ME_TRACKER + if (sp.Z == 0) { + throw vpException(vpException::badValue, "Got a point with Z == 0"); + } + if (std::isnan(sp.orientation)) { + throw vpException(vpException::badValue, "Got a point with theta nan"); + } +#endif + + if (m_useMask && frame.hasMask()) { + float confidence = frame.mask[sp.i][sp.j]; + if (confidence < m_minMaskConfidence) { + continue; + } + } + + vpRBSilhouetteControlPoint p; + p.buildPoint((int)sp.i, (int)sp.j, sp.Z, sp.orientation, sp.normal, cMo, oMc, frame.cam, m_me); + if (previousFrame.I.getSize() == frame.I.getSize()) { + p.initControlPoint(previousFrame.I, 0); + } + else { + p.initControlPoint(frame.I, 0); + } + + p.setNumCandidates(m_numCandidates); + m_controlPoints.push_back(p); + } + m_numFeatures = m_controlPoints.size(); + + m_robust.setMinMedianAbsoluteDeviation(m_robustMadMin / frame.cam.get_px()); +} + +void vpRBSilhouetteMeTracker::trackFeatures(const vpRBFeatureTrackerInput &frame, const vpRBFeatureTrackerInput &/*previousFrame*/, const vpHomogeneousMatrix &/*cMo*/) +{ + if (m_numCandidates <= 1) { + for (vpRBSilhouetteControlPoint &p: m_controlPoints) { + p.track(frame.I); + } + } + else { + for (vpRBSilhouetteControlPoint &p: m_controlPoints) { + p.trackMultipleHypotheses(frame.I); + } + } +} + +void vpRBSilhouetteMeTracker::initVVS(const vpRBFeatureTrackerInput & /*frame*/, const vpRBFeatureTrackerInput & /*previousFrame*/, const vpHomogeneousMatrix & /*cMo*/) +{ + if (m_numFeatures == 0) { + return; + } + + m_weighted_error.resize(m_numFeatures, false); + m_weights.resize(m_numFeatures, false); + m_weights = 0; + m_L.resize(m_numFeatures, 6, false, false); + m_covWeightDiag.resize(m_numFeatures, false); + m_vvsConverged = false; + m_error.resize(m_numFeatures, false); +} + +void vpRBSilhouetteMeTracker::computeVVSIter(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cMo, unsigned int /*iteration*/) +{ + vpColVector factor(m_numFeatures, 1.0); + const double threshold = m_singlePointConvergedThresholdPixels / frame.cam.get_px(); //Transformation limite pixel en limite metre. + + unsigned count = 0; + unsigned countValidSites = 0; + + for (unsigned int k = 0; k < m_controlPoints.size(); k++) { + vpRBSilhouetteControlPoint &p = m_controlPoints[k]; + //p.update(cMo); + if (m_numCandidates <= 1) { + p.computeMeInteractionMatrixError(cMo, k, m_L, m_error); + } + else { + p.computeMeInteractionMatrixErrorMH(cMo, k, m_L, m_error); + } + + m_weights[k] = 1; + if (!p.siteIsValid() || !p.isValid()) { + factor[k] = 0.0; + for (unsigned int j = 0; j < 6; j++) { + m_L[k][j] = 0; + } + } + else { + countValidSites++; + if (m_error[k] <= threshold) { + ++count; + } + } + } + + if (countValidSites == 0) { + m_vvsConverged = false; + } + else { + const double percentageConverged = (double)count / (double)countValidSites; + if (percentageConverged < m_globalVVSConvergenceThreshold) { + m_vvsConverged = false; + } + else { + m_vvsConverged = true; + } + } + + m_robust.MEstimator(vpRobust::TUKEY, m_error, m_weights); + + for (unsigned int i = 0; i < m_error.size(); i++) { + const double wi = m_weights[i] * factor[i]; + const double eri = m_error[i]; + m_covWeightDiag[i] = wi * wi; + m_weighted_error[i] = wi * eri; + for (unsigned int j = 0; j < 6; j++) { + m_L[i][j] = wi * m_L[i][j]; + } + } + + m_LTL = m_L.AtA(); + computeJTR(m_L, m_weighted_error, m_LTR); + +#if VISP_DEBUG_ME_TRACKER + for (unsigned int i = 0; i < 6; ++i) { + if (std::isnan(m_LTR[i])) { + std::cerr << m_L << std::endl; + throw vpException(vpException::badValue, "Some components were nan in ME tracker computation"); + } + } +#endif +} + +void vpRBSilhouetteMeTracker::display(const vpCameraParameters &/*cam*/, const vpImage &I, + const vpImage &/*IRGB*/, const vpImage &/*depth*/) const +{ + + for (const vpRBSilhouetteControlPoint &p: m_controlPoints) { + const vpMeSite &s = p.getSite(); + s.display(I); + } + +} + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/mask/vpColorHistogram.cpp b/modules/tracker/rbt/src/mask/vpColorHistogram.cpp new file mode 100644 index 0000000000..2fe9294451 --- /dev/null +++ b/modules/tracker/rbt/src/mask/vpColorHistogram.cpp @@ -0,0 +1,267 @@ +/* + * 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 +#include + +BEGIN_VISP_NAMESPACE + +void vpColorHistogram::Builder::build(vpColorHistogram &histogram) +{ + if (histogram.getBinNumber() != m_N) { + throw vpException(vpException::dimensionError, "Different number of bins between builder and histogram when building histogram"); + } + unsigned int count = 0; + for (unsigned int i = 0; i < m_counts.size(); ++i) { + count += m_counts[i]; + } + const float countFloat = static_cast(count); + for (unsigned int i = 0; i < m_counts.size(); ++i) { + histogram.m_probas[i] = static_cast(m_counts[i]) / countFloat; + } + histogram.m_numPixels = count; +} + +vpColorHistogram::vpColorHistogram() : m_N(0), m_binSize(0), m_numPixels(0) +{ } + +vpColorHistogram::vpColorHistogram(unsigned int N) +{ + setBinNumber(N); +} + +void vpColorHistogram::setBinNumber(unsigned int N) +{ + if (N != 1 && N != 2 && N != 4 && N != 8 && N != 16 && N != 32 && N != 64 && N != 128) { + throw vpException(vpException::badValue, "The number of bins per component should be a power of 2 (below or equal to 128)"); + } + m_N = N; + m_binSize = 256 / m_N; + m_numPixels = 0; + m_probas = std::vector(N * N * N, 0.f); +} + + + +void vpColorHistogram::build(const vpImage &image, const vpImage &mask) +{ + std::vector histo(m_N * m_N * m_N, 0); + m_probas.resize(m_N * m_N * m_N); + unsigned int pixels = 0; + for (unsigned int i = 0; i < image.getSize(); ++i) { + if (mask.bitmap[i]) { + unsigned int index = colorToIndex(image.bitmap[i]); + ++histo[index]; + ++pixels; + } + } + m_numPixels = pixels; + for (unsigned int i = 0; i < histo.size(); ++i) { + m_probas[i] = static_cast(histo[i]) / pixels; + } +} + +void vpColorHistogram::build(const std::vector &counts) +{ + if (m_probas.size() != counts.size()) { + throw vpException(vpException::dimensionError, "Number of bins are not the same"); + } + m_probas.resize(m_N * m_N * m_N); + m_numPixels = 0; + for (unsigned int count : counts) { + m_numPixels += count; + } + for (unsigned int i = 0; i < m_probas.size(); ++i) { + m_probas[i] = static_cast(counts[i]) / m_numPixels; + } +} + +void vpColorHistogram::merge(const vpColorHistogram &other, float alpha) +{ + if (other.m_N != m_N) { + throw vpException(vpException::badValue, "Histograms should have same dimensions"); + } + float malpha = 1.f - alpha; + + for (unsigned int i = 0; i < m_probas.size(); ++i) { + m_probas[i] = malpha * m_probas[i] + alpha * other.m_probas[i]; + } + +} + +void vpColorHistogram::computeProbas(const vpImage &image, vpImage &proba) const +{ + proba.resize(image.getHeight(), image.getWidth()); + +#pragma omp parallel for schedule(static) + for (unsigned int i = 0; i < image.getSize(); ++i) { + proba.bitmap[i] = m_probas[colorToIndex(image.bitmap[i])]; + } +} + +void vpColorHistogram::computeProbas(const vpImage &image, vpImage &proba, const vpRect &bb) const +{ + proba.resize(image.getHeight(), image.getWidth(), 0.f); + const int h = static_cast(image.getHeight()), w = static_cast(image.getWidth()); + const int top = static_cast(bb.getTop()); + const int left = static_cast(bb.getLeft()); + const int bottom = std::min(h- 1, static_cast(bb.getBottom())); + const int right = std::min(w - 1, static_cast(bb.getRight())); +#pragma omp parallel for + for (unsigned int i = top; i <= static_cast(bottom); ++i) { + const vpRGBa *colorRow = image[i]; + float *probaRow = proba[i]; + for (unsigned int j = left; j <= static_cast(right); ++j) { + probaRow[j] = m_probas[colorToIndex(colorRow[j])]; + } + } +} + + + +double vpColorHistogram::kl(const vpColorHistogram &other) const +{ + if (other.m_N != m_N) { + throw vpException(vpException::badValue, "Histograms should have same dimensions"); + } + double divergence = 0.0; + for (unsigned int i = 0; i < m_probas.size(); ++i) { + if (other.m_probas[i] > 0.0 && m_probas[i] > 0.0) { + divergence += m_probas[i] * log(m_probas[i] / other.m_probas[i]); + } + } + return divergence; +} + +double vpColorHistogram::jsd(const vpColorHistogram &other) const +{ + vpColorHistogram mixture(m_N); + + for (unsigned int i = 0; i < m_probas.size(); ++i) { + mixture.m_probas[i] = m_probas[i] * 0.5 + other.m_probas[i] * 0.5; + } + // JSD = 0.5KL(P || M) + 0.5(Q||M) where M is the average mixture distrib of P and Q + return (kl(mixture) + other.kl(mixture)) / 2.0; +} + +double vpColorHistogram::hellinger(const vpColorHistogram &other) const +{ + double bcoeff = 0.0; + + for (unsigned int i = 0; i < m_probas.size(); ++i) { + bcoeff += sqrt(m_probas[i] * other.m_probas[i]); + } + + return sqrt(1.0 - bcoeff); +} + +void vpColorHistogram::computeSplitHistograms(const vpImage &image, const vpImage &mask, vpColorHistogram &insideMask, vpColorHistogram &outsideMask) +{ + if (insideMask.m_N != outsideMask.m_N) { + throw vpException(vpException::badValue, "Histograms should have same number of bins"); + } + + unsigned int bins = insideMask.m_probas.size(); + + std::vector countsIn(bins, 0), countsOut(bins, 0); + +//#pragma omp parallel + { + std::vectorlocalCountsIn(bins, 0), localCountsOut(bins, 0); +//#pragma omp for schedule(static, 1024) + for (unsigned int i = 0; i < image.getSize(); ++i) { + unsigned int index = insideMask.colorToIndex(image.bitmap[i]); + localCountsIn[index] += mask.bitmap[i] > 0; + localCountsOut[index] += mask.bitmap[i] == 0; + } +//#pragma omp critical + { + for (unsigned int i = 0; i < bins; ++i) { + countsIn[i] += localCountsIn[i]; + countsOut[i] += localCountsOut[i]; + } + } + } + insideMask.build(countsIn); + outsideMask.build(countsOut); +} + +void vpColorHistogram::computeSplitHistograms(const vpImage &image, const vpImage &mask, const vpRect &bbInside, vpColorHistogram &insideMask, vpColorHistogram &outsideMask) +{ + if (insideMask.m_N != outsideMask.m_N) { + throw vpException(vpException::badValue, "Histograms should have same number of bins"); + } + + const unsigned int bins = insideMask.m_probas.size(); + + std::vector countsIn(bins, 0), countsOut(bins, 0); + + const unsigned int beforeBBStart = static_cast(bbInside.getTop()) * image.getWidth() + static_cast(bbInside.getLeft()); + const unsigned int afterBBEnd = static_cast(bbInside.getBottom()) * image.getWidth() + static_cast(bbInside.getRight()); + +#pragma omp parallel + { + std::vectorlocalCountsIn(bins, 0), localCountsOut(bins, 0); +#pragma omp for schedule(static, 64) + for (unsigned int i = 0; i < beforeBBStart; ++i) { + const unsigned int index = insideMask.colorToIndex(image.bitmap[i]); + ++localCountsOut[index]; + } +#pragma omp for schedule(static, 64) + for (unsigned int i = afterBBEnd; i < image.getSize(); ++i) { + const unsigned int index = insideMask.colorToIndex(image.bitmap[i]); + ++localCountsOut[index]; + } +#pragma omp for schedule(static, 64) + for (unsigned int i = static_cast(bbInside.getTop()); i < static_cast(round(bbInside.getBottom())); ++i) { + for (unsigned int j = static_cast(bbInside.getLeft()); j < static_cast(round(bbInside.getRight())); ++j) { + const unsigned int bitmapIndex = i * image.getWidth() + j; + const unsigned int index = insideMask.colorToIndex(image.bitmap[bitmapIndex]); + const bool pixelInMask = mask.bitmap[bitmapIndex] > 0; + localCountsIn[index] += static_cast(pixelInMask); + localCountsOut[index] += static_cast(!pixelInMask); + } + } +#pragma omp critical + { + for (unsigned int i = 0; i < bins; ++i) { + countsIn[i] += localCountsIn[i]; + countsOut[i] += localCountsOut[i]; + } + } + } + insideMask.build(countsIn); + outsideMask.build(countsOut); + +} + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/mask/vpColorHistogramMask.cpp b/modules/tracker/rbt/src/mask/vpColorHistogramMask.cpp new file mode 100644 index 0000000000..a77d0aad46 --- /dev/null +++ b/modules/tracker/rbt/src/mask/vpColorHistogramMask.cpp @@ -0,0 +1,145 @@ +/* + * 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_NLOHMANN_JSON) +#include VISP_NLOHMANN_JSON(json.hpp) +#endif + +BEGIN_VISP_NAMESPACE + +vpColorHistogramMask::vpColorHistogramMask() : m_computeOnBBOnly(false) { } + +void vpColorHistogramMask::updateMask(const vpRBFeatureTrackerInput &frame, + const vpRBFeatureTrackerInput &previousFrame, + vpImage &mask) +{ + // Prefer the last frame: + // we have updated the render to match the pose so we should get better object and background histogram separation. + const vpImage &rgb = previousFrame.IRGB.getSize() == 0 ? frame.IRGB : previousFrame.IRGB; + + const int height = static_cast(rgb.getHeight()), width = static_cast(rgb.getWidth()); + m_mask.resize(height, width, false); + const vpRect renderBB = frame.renders.boundingBox; + const int top = static_cast(renderBB.getTop()); + const int left = static_cast(renderBB.getLeft()); + const int bottom = std::min(height - 1, static_cast(renderBB.getBottom())); + const int right = std::min(width - 1, static_cast(renderBB.getRight())); + + const vpImage &renderDepth = frame.renders.depth; + const vpImage &depth = previousFrame.depth.getSize() == 0 ? frame.depth : previousFrame.depth; + if (depth.getSize() > 0 && m_depthErrorTolerance > 0.f) { + for (unsigned int i = top; i <= static_cast(bottom); ++i) { + for (unsigned int j = left; j <= static_cast(right); ++j) { + m_mask[i][j] = renderDepth[i][j] > 0.f && fabs(renderDepth[i][j] - depth[i][j]) <= m_depthErrorTolerance; + } + } + } + else { + for (unsigned int i = top; i <= static_cast(bottom); ++i) { + for (unsigned int j = left; j <= static_cast(right); ++j) { + m_mask[i][j] = renderDepth[i][j] > 0.f; + } + } + } + vpColorHistogram::computeSplitHistograms(rgb, m_mask, renderBB, m_histObjectFrame, m_histBackgroundFrame); + + const float pObject = static_cast(m_histObjectFrame.getNumPixels()) / static_cast(m_mask.getSize()); + const float pBackground = 1.f - pObject; + { + { + if (pObject != 0.f) { + m_histObject.merge(m_histObjectFrame, m_objectUpdateRate); + } + if (m_computeOnBBOnly) { + m_histObject.computeProbas(frame.IRGB, m_probaObject, frame.renders.boundingBox); + } + else { + m_histObject.computeProbas(frame.IRGB, m_probaObject); + } + } + { + if (pBackground != 0.f) { + m_histBackground.merge(m_histBackgroundFrame, m_backgroundUpdateRate); + } + if (m_computeOnBBOnly) { + m_histBackground.computeProbas(frame.IRGB, m_probaBackground, frame.renders.boundingBox); + } + else { + m_histBackground.computeProbas(frame.IRGB, m_probaBackground); + } + } + } + + if (m_computeOnBBOnly) { + mask.resize(height, width, 0.f); +#pragma omp parallel for + for (unsigned int i = top; i <= static_cast(bottom); ++i) { + for (unsigned int j = left; j <= static_cast(right); ++j) { + const float poPix = m_probaObject[i][j]; + const float pbPix = m_probaBackground[i][j]; + + float denom = (pObject * poPix + pBackground * pbPix); + mask[i][j] = (denom > 0.f) * std::max(0.f, std::min(1.f, (poPix / denom))); + m_mask[i][j] = renderDepth[i][j] > 0.f && fabs(renderDepth[i][j] - depth[i][j]) <= m_depthErrorTolerance; + } + } + } + else { + mask.resize(height, width); + for (unsigned int i = 0; i < mask.getSize(); ++i) { + // float poPix = m_histObject.probability(frame.IRGB.bitmap[i]); + // float pbPix = m_histBackground.probability(frame.IRGB.bitmap[i]); + const float poPix = m_probaObject.bitmap[i]; + const float pbPix = m_probaBackground.bitmap[i]; + + float denom = (pObject * poPix + pBackground * pbPix); + mask.bitmap[i] = (denom > 0.f) * std::max(0.f, std::min(1.f, (poPix / denom))); + } + + } + +} + +#if defined(VISP_HAVE_NLOHMANN_JSON) +void vpColorHistogramMask::loadJsonConfiguration(const nlohmann::json &json) +{ + setBinNumber(json.at("bins")); + m_backgroundUpdateRate = json.at("backgroundUpdateRate"); + m_objectUpdateRate = json.at("objectUpdateRate"); + m_depthErrorTolerance = json.at("maxDepthError"); + m_computeOnBBOnly = json.value("computeOnlyOnBoundingBox", m_computeOnBBOnly); +} +#endif + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/mask/vpObjectMask.cpp b/modules/tracker/rbt/src/mask/vpObjectMask.cpp new file mode 100644 index 0000000000..4c720a4b72 --- /dev/null +++ b/modules/tracker/rbt/src/mask/vpObjectMask.cpp @@ -0,0 +1,50 @@ +/* + * 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 + +BEGIN_VISP_NAMESPACE + +void vpObjectMask::display(const vpImage &mask, vpImage &Imask) const +{ + if (mask.getSize() != Imask.getSize()) { + throw vpException(vpException::dimensionError, "Cannot convert float mask to unsigned char mask as they do not have the same size"); + } + +#pragma omp parallel for + for (unsigned int i = 0; i < mask.getSize(); ++i) { + Imask.bitmap[i] = static_cast(mask.bitmap[i] * 255.f); + } + +} + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/mask/vpObjectMaskFactory.cpp b/modules/tracker/rbt/src/mask/vpObjectMaskFactory.cpp new file mode 100644 index 0000000000..2dc1b28545 --- /dev/null +++ b/modules/tracker/rbt/src/mask/vpObjectMaskFactory.cpp @@ -0,0 +1,49 @@ +/* + * 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 + +BEGIN_VISP_NAMESPACE + +vpObjectMaskFactory::vpObjectMaskFactory() +{ + setJsonKeyFinder([](const nlohmann::json &j) -> std::string { + return j.at("type"); + }); + + registerType("histogram", [](const nlohmann::json &j) { + std::shared_ptr p(new vpColorHistogramMask()); + p->loadJsonConfiguration(j); + return p; + }); +} + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/rendering/vpObjectCentricRenderer.cpp b/modules/tracker/rbt/src/rendering/vpObjectCentricRenderer.cpp new file mode 100644 index 0000000000..69aa79b88b --- /dev/null +++ b/modules/tracker/rbt/src/rendering/vpObjectCentricRenderer.cpp @@ -0,0 +1,191 @@ +/* + * 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 + +#include "boundingSphere.h" +#include "boundingBox.h" +#include "load_prc_file.h" + +BEGIN_VISP_NAMESPACE + +vpObjectCentricRenderer::vpObjectCentricRenderer(const vpPanda3DRenderParameters &renderParameters) + : vpPanda3DRendererSet(renderParameters), m_enableCrop(true), m_shouldComputeBBPoints(true) +{ + m_renderParameters = renderParameters; + load_prc_file_data("", "textures-power-2 none\n" + "back-buffers 0\n" + "auto-flip 1\n" + // "pstats-gpu-timing 1\n" + // "gl-finish 1\n" + // "texture-minfilter mipmap\n" + "load-file-type p3assimp\n" + "audio-library-name null\n" + "model-cache-dir\n"); +} + +void vpObjectCentricRenderer::beforeFrameRendered() +{ + if (m_shouldComputeBBPoints) { + computeBoundingBox3DPoints(); + m_shouldComputeBBPoints = false; + } + m_bb = computeBoundingBox(); + + double delta = 0.0; + m_bb.setTop(std::max(m_bb.getTop() - delta, 0.0)); + m_bb.setLeft(std::max(m_bb.getLeft() - delta, 0.0)); + m_bb.setBottom(std::min(m_bb.getBottom() + delta, (double)m_renderParameters.getImageHeight())); + m_bb.setRight(std::min(m_bb.getRight() + delta, (double)m_renderParameters.getImageWidth())); + + if (m_enableCrop) { + vpPanda3DRenderParameters subParams = m_renderParameters; + + unsigned width = (unsigned)(m_bb.getWidth()); + unsigned height = (unsigned)(m_bb.getHeight()); + + subParams.setImageResolution(height, width); + + const vpCameraParameters cam = subParams.getCameraIntrinsics(); + subParams.setCameraIntrinsics(vpCameraParameters(cam.get_px(), cam.get_py(), cam.get_u0() - m_bb.getLeft(), cam.get_v0() - m_bb.getTop())); + for (std::shared_ptr &subrenderer : m_subRenderers) { + subrenderer->setRenderParameters(subParams); + } + } +} + +void vpObjectCentricRenderer::computeBoundingBox3DPoints() +{ + if (m_subRenderers.size() == 0) { + throw vpException(vpException::fatalError, "Cannot compute bounding box with no subrender"); + } + std::shared_ptr subrenderer = m_subRenderers[0]; + NodePath object = subrenderer->getRenderRoot().find(m_focusedObject); + if (object.is_empty()) { + throw vpException(vpException::badValue, "Focused node %s was not found", m_focusedObject.c_str()); + } + m_bb3DPoints.clear(); + LPoint3 minP, maxP; + object.calc_tight_bounds(minP, maxP); + const BoundingBox box(minP, maxP); + + for (unsigned int i = 0; i < 8; ++i) { + const LPoint3 p = box.get_point(i); + m_bb3DPoints.push_back(vpColVector({ p.get_x(), -p.get_z(), p.get_y(), 1.0 })); + } +} + +void vpObjectCentricRenderer::computeClipping(float &nearV, float &farV) +{ + if (m_subRenderers.size() == 0) { + throw vpException(vpException::fatalError, "Cannot compute clpping distances with no subrenderer"); + } + std::shared_ptr subrenderer = m_subRenderers[0]; + NodePath object = subrenderer->getRenderRoot().find(m_focusedObject); + if (object.is_empty()) { + throw vpException(vpException::badValue, "Node %s was not found", m_focusedObject.c_str()); + } + if (m_shouldComputeBBPoints) { + computeBoundingBox3DPoints(); + m_shouldComputeBBPoints = false; + } + const vpHomogeneousMatrix wTcam = getCameraPose(); + const vpHomogeneousMatrix wTobj = getNodePose(m_focusedObject) * vpPanda3DBaseRenderer::PANDA_T_VISP; + const vpHomogeneousMatrix camTobj = wTcam.inverse() * wTobj; + float minZ = std::numeric_limits::max(), maxZ = 0.f; + for (unsigned int i = 0; i < m_bb3DPoints.size(); ++i) { + vpColVector cpV = camTobj * m_bb3DPoints[i]; + cpV /= cpV[3]; + float Z = cpV[2]; + if (Z > maxZ) { + maxZ = Z; + } + if (Z < minZ) { + minZ = Z; + } + } + + nearV = minZ; + farV = maxZ; +} + +vpRect vpObjectCentricRenderer::computeBoundingBox() +{ + if (m_subRenderers.size() == 0) { + throw vpException(vpException::fatalError, "Cannot compute bounding box with no subrenderer"); + } + std::shared_ptr subrenderer = m_subRenderers[0]; + NodePath object = subrenderer->getRenderRoot().find(m_focusedObject); + if (object.is_empty()) { + throw vpException(vpException::badValue, "Node %s was not found", m_focusedObject.c_str()); + } + if (m_shouldComputeBBPoints) { + computeBoundingBox3DPoints(); + m_shouldComputeBBPoints = false; + } + const auto pointToPixel = [this](const vpHomogeneousMatrix &camTobj, const vpColVector &point) -> vpImagePoint { + vpColVector cpV = camTobj * point; + cpV /= cpV[3]; + double x = cpV[0] / cpV[2]; + double y = cpV[1] / cpV[2]; + vpImagePoint ip; + vpMeterPixelConversion::convertPoint(m_renderParameters.getCameraIntrinsics(), x, y, ip); + ip.set_j(vpMath::clamp(ip.get_j(), 0.0, m_renderParameters.getImageWidth() - 1.0)); + ip.set_i(vpMath::clamp(ip.get_i(), 0.0, m_renderParameters.getImageHeight() - 1.0)); + return ip; + }; + + const vpHomogeneousMatrix wTcam = getCameraPose(); + const vpHomogeneousMatrix wTobj = getNodePose(m_focusedObject) * vpPanda3DBaseRenderer::PANDA_T_VISP; + const vpHomogeneousMatrix camTobj = wTcam.inverse() * wTobj; + + double minu = m_renderParameters.getImageWidth(), maxu = 0.0, minv = m_renderParameters.getImageHeight(), maxv = 0.0; + for (unsigned int i = 0; i < m_bb3DPoints.size(); ++i) { + const vpImagePoint ip = pointToPixel(camTobj, m_bb3DPoints[i]); + double u = ip.get_u(), v = ip.get_v(); + if (u < minu) { + minu = u; + } + if (u > maxu) { + maxu = u; + } + if (v < minv) { + minv = v; + } + if (v > maxv) { + maxv = v; + } + } + return vpRect(vpImagePoint(minv, minu), vpImagePoint(maxv, maxu)); +} + +END_VISP_NAMESPACE diff --git a/modules/tracker/rbt/src/rendering/vpPanda3DDepthFilters.cpp b/modules/tracker/rbt/src/rendering/vpPanda3DDepthFilters.cpp new file mode 100644 index 0000000000..38c08294db --- /dev/null +++ b/modules/tracker/rbt/src/rendering/vpPanda3DDepthFilters.cpp @@ -0,0 +1,248 @@ +/* + * 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) + +BEGIN_VISP_NAMESPACE + +const std::string vpPanda3DDepthGaussianBlur::FRAGMENT_SHADER = +"#version 330\n" +"\n" +"in vec2 texcoords;\n" +"\n" +"uniform sampler2D p3d_Texture0;\n" +"uniform vec2 dp; // 1 divided by number of pixels\n" +"\n" +"const float kernel[25] = float[25](\n" +" 2, 4, 5, 4, 2,\n" +" 4, 9, 12, 9, 4,\n" +" 5, 12, 15, 12, 5,\n" +" 4, 9, 12, 9, 4,\n" +" 2, 4, 5, 4, 2\n" +");\n" +"const float normalize = 1 / 159.0;\n" +"\n" +"vec2 offset[25] = vec2[25](\n" +" 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),\n" +" 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),\n" +" 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),\n" +" 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),\n" +" 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)\n" +");\n" +"\n" +"out vec4 p3d_FragData;\n" +"\n" +"void main() {\n" +" float v = 0.f;\n" +"\n" +" for(int i = 0; i < 25; ++i) {\n" +" v += kernel[i] * texture(p3d_Texture0, texcoords + offset[i]).a;\n" +" }\n" +" p3d_FragData.a = v * normalize;\n" +"}\n" +")\n"; + +vpPanda3DDepthGaussianBlur::vpPanda3DDepthGaussianBlur(const std::string &name, std::shared_ptr inputRenderer, bool isOutput) + : vpPanda3DPostProcessFilter(name, inputRenderer, isOutput, vpPanda3DDepthGaussianBlur::FRAGMENT_SHADER) +{ } + +FrameBufferProperties vpPanda3DDepthGaussianBlur::getBufferProperties() const +{ + FrameBufferProperties fbp; + fbp.set_depth_bits(0); + fbp.set_rgba_bits(0, 0, 0, 32); + fbp.set_float_color(true); + return fbp; +} + +void vpPanda3DDepthGaussianBlur::getRender(vpImage &I) const +{ + vpPanda3DPostProcessFilter::getRenderBasic(I); +} + +const std::string vpPanda3DDepthCannyFilter::FRAGMENT_SHADER = +"#version 330\n" +"\n" +"in vec2 texcoords;\n" +"\n" +"uniform sampler2D p3d_Texture0;\n" +"uniform vec2 dp; // 1 divided by number of pixels\n" +"uniform float edgeThreshold;\n" +"\n" +"const float kernel[9] = float[9](\n" +" 0.0, 1.0, 0.0,\n" +" 1.0,-4.0, 1.0,\n" +" 0.0, 1.0, 0.0\n" +");\n" +"\n" +"const float kernel_h[9] = float[9](\n" +" -1.0, 0.0, 1.0,\n" +" -2.0, 0.0, 2.0,\n" +" -1.0, 0.0, 1.0\n" +");\n" +"\n" +"const float kernel_v[9] = float[9](\n" +" -1.0, -2.0, -1.0,\n" +" 0.0, 0.0, 0.0,\n" +" 1.0, 2.0, 1.0\n" +");\n" +"\n" +"vec2 offset[9] = vec2[9](\n" +" vec2(-dp.x, -dp.y), vec2(0.0, -dp.y), vec2(dp.x, -dp.y),\n" +" vec2(-dp.x, 0.0), vec2(0.0, 0.0), vec2(dp.x, 0.0),\n" +" vec2(-dp.x, dp.y), vec2(0.0, dp.y), vec2(dp.x, dp.y)\n" +");\n" +"\n" +"float textureValues[9];\n" +"\n" +"out vec4 p3d_FragData;\n" +"\n" +"void main() {\n" +" if(texture(p3d_Texture0, texcoords).a == 0) {\n" +" p3d_FragData = vec4(0.f, 0.f, 0.f, 0.f);\n" +" } else {\n" +" float sum = 0.f;\n" +" for(int i = 0; i < 9; ++i) {\n" +" float pix = texture(p3d_Texture0, texcoords + offset[i]).a;\n" +" pix = (pix < 1e-5f ? 1000.f: pix);\n" +" textureValues[i] = pix;\n" +" sum += pix * kernel[i];\n" +" }\n" +" if(abs(sum) > edgeThreshold) {\n" +" float sum_h = 0.f;\n" +" float sum_v = 0.f;\n" +" for(int i = 0; i < 9; ++i) {\n" +" float pix = textureValues[i];\n" +" sum_h += pix * kernel_h[i];\n" +" sum_v += pix * kernel_v[i];\n" +" }\n" +" float norm = sqrt(sum_v * sum_v + sum_h * sum_h);\n" +" vec2 orientationAndValid = (sum_h != 0.f) ? vec2(atan(sum_v, -sum_h), 1.f) : vec2(0.f, 0.f);\n" +" p3d_FragData.bgra = vec4(sum_h, sum_v, orientationAndValid.x, orientationAndValid.y);\n" +" } else {\n" +" p3d_FragData = vec4(0.f, 0.f, 0.f, 0.f);\n" +" }\n" +" }\n" +"}\n"; + +vpPanda3DDepthCannyFilter::vpPanda3DDepthCannyFilter(const std::string &name, std::shared_ptr inputRenderer, bool isOutput, float edgeThreshold) + : vpPanda3DPostProcessFilter(name, inputRenderer, isOutput, vpPanda3DDepthCannyFilter::FRAGMENT_SHADER), m_edgeThreshold(edgeThreshold) +{ } + +void vpPanda3DDepthCannyFilter::setupScene() +{ + vpPanda3DPostProcessFilter::setupScene(); + m_renderRoot.set_shader_input("edgeThreshold", LVector2f(m_edgeThreshold)); +} + +void vpPanda3DDepthCannyFilter::setEdgeThreshold(float edgeThreshold) +{ + m_edgeThreshold = edgeThreshold; + m_renderRoot.set_shader_input("edgeThreshold", LVector2f(m_edgeThreshold)); +} + + +FrameBufferProperties vpPanda3DDepthCannyFilter::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 vpPanda3DDepthCannyFilter::getRender(vpImage &I, vpImage &valid) 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()); + + valid.resize(I.getHeight(), I.getWidth()); + 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; + if (numComponents != 4) { + throw; + } + for (unsigned int i = 0; i < I.getHeight(); ++i) { + vpRGBf *colorRow = I[i]; + unsigned char *validRow = valid[i]; + for (unsigned int j = 0; j < I.getWidth(); ++j) { + colorRow[j].R = data[j * numComponents]; + colorRow[j].G = data[j * numComponents + 1]; + colorRow[j].B = data[j * numComponents + 2]; + validRow[j] = static_cast(data[j * numComponents + 3]); + } + data += rowIncrement; + } +} + +void vpPanda3DDepthCannyFilter::getRender(vpImage &I, vpImage &valid, const vpRect &bb, unsigned int h, unsigned w) 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(h, w, 0.f); + valid.resize(I.getHeight(), I.getWidth(), 0); + + const unsigned top = static_cast(std::max(0.0, bb.getTop())); + const unsigned left = static_cast(std::max(0.0, bb.getLeft())); + const unsigned numComponents = m_texture->get_num_components(); + const unsigned rowIncrement = m_renderParameters.getImageWidth() * numComponents; // we ask for only 8 bits image, but we may get an rgb image + + const float *data = (float *)(&(m_texture->get_ram_image().front())); + data += rowIncrement * (m_renderParameters.getImageHeight() - 1); + if (numComponents != 4) { + throw vpException(vpException::dimensionError, "Expected panda texture to have 4 components!"); + } + for (unsigned int i = 0; i < m_renderParameters.getImageHeight(); ++i) { + const float *rowData = data - i * rowIncrement; + vpRGBf *colorRow = I[top + i]; + unsigned char *validRow = valid[top + i]; + for (unsigned int j = 0; j < m_renderParameters.getImageWidth(); ++j) { + colorRow[left + j].R = rowData[j * numComponents]; + colorRow[left + j].G = rowData[j * numComponents + 1]; + colorRow[left + j].B = rowData[j * numComponents + 2]; + validRow[left + j] = static_cast(rowData[j * numComponents + 3]); + } + } +} + +END_VISP_NAMESPACE + +#endif diff --git a/modules/tracker/rbt/src/vo/vpRBVisualOdometry.cpp b/modules/tracker/rbt/src/vo/vpRBVisualOdometry.cpp new file mode 100644 index 0000000000..f13befe401 --- /dev/null +++ b/modules/tracker/rbt/src/vo/vpRBVisualOdometry.cpp @@ -0,0 +1,3 @@ +#include + +vpRBVisualOdometry::vpRBVisualOdometry() { } diff --git a/modules/tracker/rbt/test/catchRBT.cpp b/modules/tracker/rbt/test/catchRBT.cpp new file mode 100644 index 0000000000..fd3f65632e --- /dev/null +++ b/modules/tracker/rbt/test/catchRBT.cpp @@ -0,0 +1,711 @@ +/* + * 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: + * Test vpMbGenericTracker JSON parse / save. + */ + +/*! + \file testMbtJsonSettings.cpp + + Test test saving and parsing JSON configuration for vpMbGenericTracker +*/ + +#include + +#if defined(VISP_HAVE_CATCH2) + +#include +#include +#include + +#include +#include +#include +#include + +#include "test_utils.h" + +#if defined(VISP_HAVE_NLOHMANN_JSON) +#include VISP_NLOHMANN_JSON(json.hpp) +#endif + +#define CATCH_CONFIG_RUNNER +#include + +const std::string objCube = +"o Cube\n" +"v -0.050000 -0.050000 0.050000\n" +"v -0.050000 0.050000 0.050000\n" +"v -0.050000 -0.050000 -0.050000\n" +"v -0.050000 0.050000 -0.050000\n" +"v 0.050000 -0.050000 0.050000\n" +"v 0.050000 0.050000 0.050000\n" +"v 0.050000 -0.050000 -0.050000\n" +"v 0.050000 0.050000 -0.050000\n" +"vn -1.0000 -0.0000 -0.0000\n" +"vn -0.0000 -0.0000 -1.0000\n" +"vn 1.0000 -0.0000 -0.0000\n" +"vn -0.0000 -0.0000 1.0000\n" +"vn -0.0000 -1.0000 -0.0000\n" +"vn -0.0000 1.0000 -0.0000\n" +"vt 0.375000 0.000000\n" +"vt 0.375000 1.000000\n" +"vt 0.125000 0.750000\n" +"vt 0.625000 0.000000\n" +"vt 0.625000 1.000000\n" +"vt 0.875000 0.750000\n" +"vt 0.125000 0.500000\n" +"vt 0.375000 0.250000\n" +"vt 0.625000 0.250000\n" +"vt 0.875000 0.500000\n" +"vt 0.375000 0.750000\n" +"vt 0.625000 0.750000\n" +"vt 0.375000 0.500000\n" +"vt 0.625000 0.500000\n" +"s 0\n" +"f 2/4/1 3/8/1 1/1/1\n" +"f 4/9/2 7/13/2 3/8/2\n" +"f 8/14/3 5/11/3 7/13/3\n" +"f 6/12/4 1/2/4 5/11/4\n" +"f 7/13/5 1/3/5 3/7/5\n" +"f 4/10/6 6/12/6 8/14/6\n" +"f 2/4/1 4/9/1 3/8/1\n" +"f 4/9/2 8/14/2 7/13/2\n" +"f 8/14/3 6/12/3 5/11/3\n" +"f 6/12/4 2/5/4 1/2/4\n" +"f 7/13/5 5/11/5 1/3/5\n" +"f 4/10/6 2/6/6 6/12/6\n"; + +SCENARIO("Instantiating a silhouette me tracker", "[rbt]") +{ + GIVEN("A base me tracker") + { + vpRBSilhouetteMeTracker tracker; + WHEN("Changing mask parameters") + { + THEN("Enabling mask is seen") + { + bool useMaskDefault = tracker.shouldUseMask(); + tracker.setShouldUseMask(!useMaskDefault); + REQUIRE(useMaskDefault != tracker.shouldUseMask()); + } + THEN("Changing mask min confidence with a correct value is Ok") + { + tracker.setMinimumMaskConfidence(0.0); + REQUIRE(tracker.getMinimumMaskConfidence() == 0.0); + tracker.setMinimumMaskConfidence(1.0); + REQUIRE(tracker.getMinimumMaskConfidence() == 1.0); + tracker.setMinimumMaskConfidence(0.5); + REQUIRE(tracker.getMinimumMaskConfidence() == 0.5); + } + THEN("Setting incorrect mask confidence value fails") + { + REQUIRE_THROWS(tracker.setMinimumMaskConfidence(-1.0)); + } + } + WHEN("Changing robust threshold") + { + THEN("Setting correct value works") + { + tracker.setMinRobustThreshold(0.5); + REQUIRE(tracker.getMinRobustThreshold() == 0.5); + } + THEN("Setting negative value throws") + { + REQUIRE_THROWS(tracker.setMinRobustThreshold(-0.5)); + } + } + WHEN("Changing number of candidates") + { + THEN("Setting correct value works") + { + tracker.setNumCandidates(3); + REQUIRE(tracker.getNumCandidates() == 3); + } + THEN("Setting incorrect value throws") + { + REQUIRE_THROWS(tracker.setNumCandidates(0)); + } + } + WHEN("Changing convergence settings") + { + THEN("Setting correct single point value works") + { + tracker.setSinglePointConvergenceThreshold(1.0); + REQUIRE(tracker.getSinglePointConvergenceThreshold() == 1.0); + } + THEN("Setting incorrect single point value throws") + { + REQUIRE_THROWS(tracker.setSinglePointConvergenceThreshold(-1.0)); + } + THEN("Setting correct global value works") + { + tracker.setGlobalConvergenceMinimumRatio(0.0); + REQUIRE(tracker.getGlobalConvergenceMinimumRatio() == 0.0); + tracker.setGlobalConvergenceMinimumRatio(1.0); + REQUIRE(tracker.getGlobalConvergenceMinimumRatio() == 1.0); + tracker.setGlobalConvergenceMinimumRatio(0.5); + REQUIRE(tracker.getGlobalConvergenceMinimumRatio() == 0.5); + } + } +#if defined(VISP_HAVE_NLOHMANN_JSON) + WHEN("defining JSON parameters") + { + nlohmann::json j = { + {"type", "silhouetteMe"}, + { "numCandidates", 1 }, + { "weight", 0.5 }, + { "convergencePixelThreshold", 0.5 }, + { "convergenceRatio", 0.99}, + { "useMask", true}, + { "minMaskConfidence", 0.5}, + { "movingEdge", { + {"maskSign", 0}, + {"maskSize" , 5}, + {"minSampleStep" , 4.0}, + {"mu" , {0.5, 0.5}}, + {"nMask" , 90}, + {"ntotalSample" , 0}, + {"pointsToTrack" , 200}, + {"range" , 5}, + {"sampleStep" , 4.0}, + {"strip" , 2}, + {"thresholdType" , "normalized"}, + {"threshold" , 20.0} + }} + }; + THEN("Loading correct settings works") + { + tracker.loadJsonConfiguration(j); + REQUIRE(tracker.getNumCandidates() == 1); + REQUIRE(tracker.shouldUseMask() == true); + REQUIRE(tracker.getMinimumMaskConfidence() == 0.5); + REQUIRE(tracker.getMe().getMaskNumber() == 90); + REQUIRE(tracker.getMe().getThreshold() == 20.0); + } + THEN("Setting incorrect candidate number throws") + { + j["numCandidates"] = 0; + REQUIRE_THROWS(tracker.loadJsonConfiguration(j)); + } + THEN("Setting incorrect mask confidence throws") + { + j["minMaskConfidence"] = 5.0; + REQUIRE_THROWS(tracker.loadJsonConfiguration(j)); + } + THEN("Setting incorrect single point convergence vlaue confidence throws") + { + j["convergencePixelThreshold"] = -1.0; + REQUIRE_THROWS(tracker.loadJsonConfiguration(j)); + } + THEN("Setting incorrect global convergence vlaue confidence throws") + { + j["convergenceRatio"] = 2.0; + REQUIRE_THROWS(tracker.loadJsonConfiguration(j)); + } + } + } +#endif +} + +SCENARIO("Instantiating a silhouette CCD tracker", "[rbt]") +{ + vpRBSilhouetteCCDTracker tracker; + WHEN("Setting smoothing factor") + { + THEN("Setting value above 0 works") + { + tracker.setTemporalSmoothingFactor(0.5); + REQUIRE(tracker.getTemporalSmoothingFactor() == 0.5); + } + THEN("Setting value below 0 throws") + { + REQUIRE_THROWS(tracker.setTemporalSmoothingFactor(-2.0)); + } + } + WHEN("Updating CCD parameters") + { + vpCCDParameters ccd = tracker.getCCDParameters(); + ccd.h += 4; + ccd.delta_h += 2; + tracker.setCCDParameters(ccd); + THEN("Changes are propagated to tracker") + { + REQUIRE(tracker.getCCDParameters().h == ccd.h); + REQUIRE(tracker.getCCDParameters().delta_h == ccd.delta_h); + } + + } + +#if defined(VISP_HAVE_NLOHMANN_JSON) + WHEN("Defining associated json") + { + nlohmann::json j = { + {"type", "silhouetteCCD"}, + {"weight", 0.01}, + {"temporalSmoothing", 0.1}, + {"convergenceThreshold", 0.1}, + {"ccd", { + {"h", 64}, + {"delta_h", 16}, + {"gamma", { 0.1, 0.2, 0.3, 0.4 } } + }} + }; + THEN("Loading correct json works") + { + tracker.loadJsonConfiguration(j); + REQUIRE(tracker.getTemporalSmoothingFactor() == 0.1); + vpCCDParameters ccd = tracker.getCCDParameters(); + REQUIRE(ccd.h == 64); + REQUIRE(ccd.delta_h == 16); + REQUIRE((ccd.gamma_1 == 0.1 && ccd.gamma_2 == 0.2 && ccd.gamma_3 == 0.3 && ccd.gamma_4 == 0.4)); + } + THEN("Loading invalid temporal smoothing factor throws") + { + j["temporalSmoothing"] = -3.14; + REQUIRE_THROWS(tracker.loadJsonConfiguration(j)); + } + THEN("Loading invalid ccd gamma throws") + { + j["ccd"]["gamma"] = -3.14; + REQUIRE_THROWS(tracker.loadJsonConfiguration(j)); + } + } +#endif +} + +#if defined(VP_HAVE_RB_KLT_TRACKER) +SCENARIO("Instantiating KLT tracker") +{ + vpRBKltTracker tracker; + WHEN("Modifying basic settings") + { + tracker.setFilteringBorderSize(2); + tracker.setFilteringMaxReprojectionError(0.024); + tracker.setMinimumDistanceNewPoints(0.005); + tracker.setMinimumNumberOfPoints(20); + tracker.setShouldUseMask(true); + tracker.setMinimumMaskConfidence(0.5); + THEN("Every change is visible") + { + REQUIRE(tracker.getFilteringBorderSize() == 2); + REQUIRE(tracker.getFilteringMaxReprojectionError() == 0.024); + REQUIRE(tracker.getMinimumDistanceNewPoints() == 0.005); + REQUIRE(tracker.getMinimumNumberOfPoints() == 20); + REQUIRE(tracker.shouldUseMask()); + REQUIRE(tracker.getMinimumMaskConfidence() == 0.5); + } + THEN("Setting incorrect Mask confidence throws") + { + REQUIRE_THROWS(tracker.setMinimumMaskConfidence(-1.0)); + } + } + +#if defined(VISP_HAVE_NLOHMANN_JSON) + WHEN("Defining associated json") + { + nlohmann::json j = { + {"type", "klt"}, + {"weight", 0.01}, + {"minimumNumPoints", 25}, + {"newPointsMinPixelDistance", 5}, + {"maxReprojectionErrorPixels", 0.01}, + {"useMask", true}, + {"minMaskConfidence", 0.1}, + { "windowSize", 7 }, + { "quality", 0.01 }, + { "maxFeatures", 500 } + }; + THEN("Loading correct json works") + { + tracker.loadJsonConfiguration(j); + REQUIRE(tracker.getMinimumNumberOfPoints() == 25); + REQUIRE(tracker.getMinimumDistanceNewPoints() == 5); + REQUIRE(tracker.getFilteringMaxReprojectionError() == 0.01); + REQUIRE(tracker.shouldUseMask() == true); + REQUIRE(tracker.getMinimumMaskConfidence() == 0.1f); + REQUIRE(tracker.getKltTracker().getWindowSize() == 7); + REQUIRE(tracker.getKltTracker().getQuality() == 0.01); + REQUIRE(tracker.getKltTracker().getMaxFeatures() == 500); + } + THEN("Loading invalid mask confidence throws") + { + j["minMaskConfidence"] = -3.14; + REQUIRE_THROWS(tracker.loadJsonConfiguration(j)); + } + } +#endif +} +#endif + +SCENARIO("Instantiating depth tracker", "[rbt]") +{ + vpRBDenseDepthTracker tracker; + WHEN("Setting steps") + { + THEN("Setting positive value works") + { + tracker.setStep(4); + REQUIRE(tracker.getStep() == 4); + } + THEN("Setting 0 step is invalid") + { + REQUIRE_THROWS(tracker.setStep(0)); + } + } + WHEN("Setting confidence") + { + THEN("Setting incorrect mask confidence value") + { + REQUIRE_THROWS(tracker.setMinimumMaskConfidence(-1.0)); + } + THEN("Setting correct mask confidence value") + { + tracker.setMinimumMaskConfidence(0.8); + REQUIRE(tracker.getMinimumMaskConfidence() == 0.8f); + } + THEN("Toggling mask works") + { + tracker.setShouldUseMask(true); + REQUIRE(tracker.shouldUseMask()); + } + } +#if defined(VISP_HAVE_NLOHMANN_JSON) + WHEN("Defining associated json") + { + nlohmann::json j = { + {"type", "klt"}, + {"weight", 0.01}, + {"step", 16}, + {"useMask", true}, + {"minMaskConfidence", 0.1} + }; + THEN("Loading correct json works") + { + tracker.loadJsonConfiguration(j); + REQUIRE(tracker.getStep() == 16); + REQUIRE(tracker.shouldUseMask()); + REQUIRE(tracker.getMinimumMaskConfidence() == 0.1f); + } + THEN("Loading invalid mask confidence throws") + { + j["minMaskConfidence"] = -3.14; + REQUIRE_THROWS(tracker.loadJsonConfiguration(j)); + } + THEN("Loading invalid step throws") + { + j["step"] = 0; + REQUIRE_THROWS(tracker.loadJsonConfiguration(j)); + } + } +#endif +} + +SCENARIO("Instantiating a render-based tracker", "[rbt]") +{ + vpRBTracker tracker; + + WHEN("Setting optimization parameters") + { + THEN("Max num iter cannot be zero") + { + REQUIRE_THROWS(tracker.setMaxOptimizationIters(0)); + } + THEN("Setting num iter is ok") + { + tracker.setMaxOptimizationIters(10); + REQUIRE(tracker.getMaxOptimizationIters() == 10); + } + THEN("Gain cannot be negative") + { + REQUIRE_THROWS(tracker.setOptimizationGain(-0.5)); + } + THEN("Positive gain is ok") + { + tracker.setOptimizationGain(0.5); + REQUIRE(tracker.getOptimizationGain() == 0.5); + } + THEN("Initial mu cannot be negative") + { + REQUIRE_THROWS(tracker.setOptimizationInitialMu(-0.5)); + } + THEN("Initial mu can be zero (gauss newton)") + { + tracker.setOptimizationInitialMu(0.0); + REQUIRE(tracker.getOptimizationInitialMu() == 0.0); + } + THEN("Initial mu can be above zero") + { + tracker.setOptimizationInitialMu(0.1); + REQUIRE(tracker.getOptimizationInitialMu() == 0.1); + } + + THEN("Mu factor cannot be negative") + { + REQUIRE_THROWS(tracker.setOptimizationMuIterFactor(-0.5)); + } + THEN("Mu factor can be zero") + { + tracker.setOptimizationMuIterFactor(0.0); + REQUIRE(tracker.getOptimizationMuIterFactor() == 0.0); + } + THEN("Mu factor can be positive") + { + tracker.setOptimizationMuIterFactor(0.1); + REQUIRE(tracker.getOptimizationMuIterFactor() == 0.1); + } + } + + WHEN("Setting camera parameters and resolution") + { + unsigned int h = 480, w = 640; + vpCameraParameters cam(600, 600, 320, 240); + THEN("Image height cannot be zero") + { + REQUIRE_THROWS(tracker.setCameraParameters(cam, 0, w)); + } + THEN("Image width cannot be zero") + { + REQUIRE_THROWS(tracker.setCameraParameters(cam, h, 0)); + } + THEN("Camera model cannot have distortion") + { + cam.initPersProjWithDistortion(600, 600, 320, 240, 0.01, 0.01); + REQUIRE_THROWS(tracker.setCameraParameters(cam, h, w)); + } + THEN("Loading with perspective model with no distortion and correct resolution is ok") + { + tracker.setCameraParameters(cam, h, w); + REQUIRE(tracker.getCameraParameters() == cam); + REQUIRE(tracker.getImageHeight() == h); + REQUIRE(tracker.getImageWidth() == w); + } + } + +#if defined(VISP_HAVE_NLOHMANN_JSON) + WHEN("Loading JSON configuration") + { + const std::string jsonLiteral = R"JSON({ + "camera": { + "intrinsics": { + "model": "perspectiveWithoutDistortion", + "px" : 302.573, + "py" : 302.396, + "u0" : 162.776, + "v0" : 122.475 + }, + "height": 240, + "width" : 320 + }, + "vvs": { + "gain": 1.0, + "maxIterations" : 10, + "mu": 0.5, + "muIterFactor": 0.1 + }, + "model" : "path/to/model.obj", + "silhouetteExtractionSettings" : { + "threshold": { + "type": "relative", + "value" : 0.1 + }, + "sampling" : { + "type": "fixed", + "samplingRate": 2, + "numPoints" : 128, + "reusePreviousPoints": true + } + }, + "features": [ + { + "type": "silhouetteMe", + "weight" : 0.5, + "numCandidates" : 3, + "convergencePixelThreshold" : 3, + "convergenceRatio" : 0.99, + "movingEdge" : { + "maskSign": 0, + "maskSize" : 5, + "minSampleStep" : 4.0, + "mu" : [ + 0.5, + 0.5 + ] , + "nMask" : 90, + "ntotalSample" : 0, + "pointsToTrack" : 200, + "range" : 5, + "sampleStep" : 4.0, + "strip" : 2, + "thresholdType" : "normalized", + "threshold" : 20.0 + } + }, + { + "type": "silhouetteColor", + "weight" : 0.5, + "convergenceThreshold" : 0.1, + "temporalSmoothing" : 0.1, + "ccd" : { + "h": 4, + "delta_h" : 1 + } + } + ] + })JSON"; + const auto verifyBase = [&tracker]() { + REQUIRE((tracker.getImageHeight() == 240 && tracker.getImageWidth() == 320)); + REQUIRE((tracker.getOptimizationGain() == 1.0 && tracker.getMaxOptimizationIters() == 10)); + vpSilhouettePointsExtractionSettings silset = tracker.getSilhouetteExtractionParameters(); + REQUIRE((silset.thresholdIsRelative() && silset.getThreshold() == 0.1)); + REQUIRE((silset.getSampleStep() == 2 && silset.getMaxCandidates() == 128)); + REQUIRE((silset.preferPreviousPoints())); + + REQUIRE((tracker.getOptimizationGain() == 1.0 && tracker.getMaxOptimizationIters() == 10)); + REQUIRE((tracker.getOptimizationInitialMu() == 0.5 && tracker.getOptimizationMuIterFactor() == 0.1)); + }; + nlohmann::json j = nlohmann::json::parse(jsonLiteral); + THEN("Loading configuration with trackers") + { + tracker.loadConfiguration(j); + verifyBase(); + REQUIRE(tracker.getModelPath() == "path/to/model.obj"); + AND_THEN("Initializing tracking fails since object does not exist") + { + REQUIRE_THROWS(tracker.startTracking()); + } + } + THEN("Loading configuration without model also works") + { + j.erase("model"); + tracker.loadConfiguration(j); + verifyBase(); + REQUIRE(tracker.getModelPath() == ""); + AND_THEN("Initializing tracking fails since path is not specified") + { + REQUIRE_THROWS(tracker.startTracking()); + } + } + THEN("Loading configuration with real 3D model also works") + { + const std::string tempDir = vpIoTools::makeTempDirectory("visp_test_rbt_obj"); + const std::string objFile = vpIoTools::createFilePath(tempDir, "cube.obj"); + std::ofstream f(objFile); + f << objCube; + f.close(); + j["model"] = objFile; + tracker.loadConfiguration(j); + verifyBase(); + REQUIRE(tracker.getModelPath() == objFile); + AND_THEN("Initializing tracker works") + { + REQUIRE_NOTHROW(tracker.startTracking()); + } + } + } + + WHEN("Adding trackers") + { + THEN("Adding nullptr is not allowed") + { + REQUIRE_THROWS(tracker.addTracker(nullptr)); + } + THEN("Adding a tracker works") + { + auto ccdTracker = std::make_shared(); + tracker.addTracker(ccdTracker); + } + } +#endif +} + +SCENARIO("Running tracker on static synthetic sequences", "[rbt]") +{ + vpRBTracker tracker; + unsigned int h = 240, w = 320; + vpCameraParameters cam(300, 300, 160, 120); + vpPanda3DRenderParameters renderParams(cam, h, w, 0.01, 1.0); + + const std::string tempDir = vpIoTools::makeTempDirectory("visp_test_rbt_obj"); + const std::string objFile = vpIoTools::createFilePath(tempDir, "cube.obj"); + std::ofstream f(objFile); + f << objCube; + f.close(); + + const auto setupScene = [&objFile](vpPanda3DRendererSet &renderer) { + renderer.addNodeToScene(renderer.loadObject("object", objFile)); + renderer.addLight(vpPanda3DAmbientLight("ambient", vpRGBf(1.f))); + }; + const unsigned int n = 100; + std::vector cTw; + std::vector oTw; + for (unsigned int i = 0; i < n; ++i) { + oTw.push_back(vpHomogeneousMatrix(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + cTw.push_back(vpHomogeneousMatrix(0.0, 0.0, 0.3, 0.0, 0.0, 0.0)); + } + + TrajectoryData traj1 = generateTrajectory(renderParams, setupScene, cTw, oTw); + + tracker.addTracker(std::make_shared()); + tracker.setModelPath(objFile); + tracker.startTracking(); + + tracker.setPose(traj1.cTo[0]); + vpImage I; + std::cout << "Running tracker" << std::endl; + for (unsigned int i = 0; i < traj1.cTo.size(); ++i) { + vpImageConvert::convert(traj1.rgb[i], I); + tracker.track(I, traj1.rgb[i], traj1.depth[i]); + vpHomogeneousMatrix tracker_cTo; + tracker.getPose(tracker_cTo); + vpHomogeneousMatrix cdTc = traj1.cTo[i] * tracker_cTo.inverse(); + double errorT = cdTc.getTranslationVector().frobeniusNorm(); + double errorR = cdTc.getThetaUVector().getTheta(); + REQUIRE((errorT < 0.005 && errorR < vpMath::rad(0.5))); + } +} + +int main(int argc, char *argv[]) +{ + Catch::Session session; // There must be exactly one instance + session.applyCommandLine(argc, argv); + + int numFailed = session.run(); + return numFailed; +} + +#else + +int main() +{ + return EXIT_SUCCESS; +} + +#endif diff --git a/modules/tracker/rbt/test/test_utils.h b/modules/tracker/rbt/test/test_utils.h new file mode 100644 index 0000000000..aea610ecca --- /dev/null +++ b/modules/tracker/rbt/test/test_utils.h @@ -0,0 +1,56 @@ +#ifndef VP_RB_TEST_UTILS_H +#define VP_RB_TEST_UTILS_H + +#include +#include +#include + +#include + +struct TrajectoryData +{ + std::vector> rgb; + std::vector> depth; + std::vector cTo; +}; + +TrajectoryData generateTrajectory(const vpPanda3DRenderParameters &renderingParams, + const std::function &makeScene, + std::vector &cTw, std::vector &oTw) +{ + vpPanda3DRendererSet renderer(renderingParams); + auto rgbRenderer = std::make_shared(true); + renderer.addSubRenderer(rgbRenderer); + renderer.addSubRenderer(std::make_shared(vpPanda3DGeometryRenderer::OBJECT_NORMALS)); + renderer.initFramework(); + makeScene(renderer); + + if (cTw.size() != oTw.size()) { + throw vpException(vpException::dimensionError, "Number of poses don't match"); + } + TrajectoryData res; + res.rgb.resize(cTw.size()); + res.depth.resize(cTw.size()); + res.cTo.resize(cTw.size()); + + for (unsigned int i = 0; i < cTw.size(); ++i) { + res.rgb[i].resize(renderingParams.getImageHeight(), renderingParams.getImageWidth()); + res.depth[i].resize(renderingParams.getImageHeight(), renderingParams.getImageWidth()); + renderer.setNodePose("object", oTw[i].inverse()); + renderer.setCameraPose(cTw[i].inverse()); + + float nearV, farV; + rgbRenderer->computeNearAndFarPlanesFromNode("object", nearV, farV, true); + vpPanda3DRenderParameters renderingParamsFrame = renderingParams; + renderingParamsFrame.setClippingDistance(nearV, farV); + renderer.setRenderParameters(renderingParamsFrame); + // update clip + renderer.renderFrame(); + renderer.getRenderer()->getRender(res.rgb[i]); + renderer.getRenderer()->getRender(res.depth[i]); + res.cTo[i] = cTw[i] * oTw[i].inverse(); + } + return res; +} + +#endif diff --git a/modules/vision/src/key-point/vpKeyPoint.cpp b/modules/vision/src/key-point/vpKeyPoint.cpp index ac7965418d..1433cf0c4e 100644 --- a/modules/vision/src/key-point/vpKeyPoint.cpp +++ b/modules/vision/src/key-point/vpKeyPoint.cpp @@ -3857,7 +3857,7 @@ void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode, int class_id = m_trainKeyPoints[i_].class_id; vpIoTools::writeBinaryValueLE(file, class_id); -// Write image_id + // Write image_id #ifdef VISP_HAVE_MODULE_IO std::map::const_iterator it_findImgId = m_mapOfImageId.find(m_trainKeyPoints[i_].class_id); int image_id = (saveTrainingImages && it_findImgId != m_mapOfImageId.end()) ? it_findImgId->second : -1; diff --git a/script/make-coverage-report.sh b/script/make-coverage-report.sh old mode 100644 new mode 100755 index c74d476d7a..9d19d10e46 --- a/script/make-coverage-report.sh +++ b/script/make-coverage-report.sh @@ -18,7 +18,7 @@ fi cd $build_dir lcov --zerocounters --directory . -cmake $source_dir -DBUILD_COVERAGE=ON -DBUILD_DEPRECATED_FUNCTIONS=OFF +cmake $source_dir -DBUILD_COVERAGE=ON -DBUILD_DEPRECATED_FUNCTIONS=OFF -DCMAKE_BUILD_TYPE=Debug cmake --build . --target all -j$(nproc) cmake --build . --target test -j$(nproc) lcov --directory . --capture --output-file visp-coverage.info diff --git a/tutorial/CMakeLists.txt b/tutorial/CMakeLists.txt index 33a706700d..e42d8546df 100644 --- a/tutorial/CMakeLists.txt +++ b/tutorial/CMakeLists.txt @@ -68,6 +68,8 @@ visp_add_subdirectory(tracking/model-based/old/edges REQUIRED_DEPS visp_co visp_add_subdirectory(tracking/model-based/old/generic REQUIRED_DEPS visp_core visp_mbt visp_io visp_gui) visp_add_subdirectory(tracking/model-based/old/hybrid REQUIRED_DEPS visp_core visp_mbt visp_klt visp_io visp_gui) visp_add_subdirectory(tracking/model-based/old/keypoint REQUIRED_DEPS visp_core visp_mbt visp_klt visp_io visp_gui) +visp_add_subdirectory(tracking/render-based REQUIRED_DEPS visp_core visp_rbt visp_io visp_gui visp_sensor) + visp_add_subdirectory(tracking/template-tracker REQUIRED_DEPS visp_core visp_tt visp_io visp_gui) visp_add_subdirectory(tracking/moving-edges REQUIRED_DEPS visp_core visp_me visp_io visp_gui visp_sensor) visp_add_subdirectory(tracking/dnn REQUIRED_DEPS visp_core visp_detection visp_dnn_tracker visp_io visp_gui visp_sensor) diff --git a/tutorial/ar/tutorial-panda3d-renderer.cpp b/tutorial/ar/tutorial-panda3d-renderer.cpp index 617938e784..6cdb70564c 100644 --- a/tutorial/ar/tutorial-panda3d-renderer.cpp +++ b/tutorial/ar/tutorial-panda3d-renderer.cpp @@ -160,7 +160,7 @@ int main(int argc, const char **argv) double factor = 1.0; vpPanda3DRenderParameters renderParams(vpCameraParameters(600 * factor, 600 * factor, 320 * factor, 240 * factor), int(480 * factor), int(640 * factor), 0.01, 10.0); unsigned h = renderParams.getImageHeight(), w = renderParams.getImageWidth(); - vpPanda3DRendererSet renderer(renderParams); + vpPanda3DRendererSet renderer; renderer.setRenderParameters(renderParams); renderer.setVerticalSyncEnabled(false); renderer.setAbortOnPandaError(true); diff --git a/tutorial/tracking/render-based/CMakeLists.txt b/tutorial/tracking/render-based/CMakeLists.txt new file mode 100644 index 0000000000..6debb62258 --- /dev/null +++ b/tutorial/tracking/render-based/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.10) + +project(tutorial-render-based-tracking) + +find_package(VISP REQUIRED visp_core visp_rbt visp_io visp_gui visp_sensor) + +# set the list of source files +set(tutorial_cpp + tutorial-rbt-sequence.cpp + tutorial-rbt-realsense.cpp) + +list(APPEND tutorial_data_dir "data") + +foreach(cpp ${tutorial_cpp}) + visp_add_target(${cpp}) + + if(VISP_HAVE_OGRE) + # Add specific build flag to turn off warnings coming from Ogre3D 3rd party + list(APPEND CXX_FLAGS_MUTE_WARNINGS "-Wno-deprecated-copy") + list(APPEND CXX_FLAGS_MUTE_WARNINGS "-Wno-float-equal") + list(APPEND CXX_FLAGS_MUTE_WARNINGS "-Wno-ignored-qualifiers") + list(APPEND CXX_FLAGS_MUTE_WARNINGS "-Wno-overloaded-virtual") + list(APPEND CXX_FLAGS_MUTE_WARNINGS "-Wno-register") + list(APPEND CXX_FLAGS_MUTE_WARNINGS "-Wno-unused-parameter") + list(APPEND CXX_FLAGS_MUTE_WARNINGS "-Wno-unused-but-set-parameter") + endif() + + if(CXX_FLAGS_MUTE_WARNINGS) + # Add specific build flag to turn off warnings + visp_set_source_file_compile_flag(${cpp} ${CXX_FLAGS_MUTE_WARNINGS}) + endif() + + if(COMMAND visp_add_dependency) + visp_add_dependency(${cpp} "tutorials") + endif() +endforeach() + +# Copy the data files to the same location than the target +foreach(data_dir ${tutorial_data_dir}) + visp_copy_dir(tutorial-rbt-sequence.cpp "${CMAKE_CURRENT_SOURCE_DIR}" ${data_dir}) + visp_copy_dir(tutorial-rbt-realsense.cpp "${CMAKE_CURRENT_SOURCE_DIR}" ${data_dir}) +endforeach() diff --git a/tutorial/tracking/render-based/data/sequence1/dragon.0.pos b/tutorial/tracking/render-based/data/sequence1/dragon.0.pos new file mode 100644 index 0000000000..04e1697184 --- /dev/null +++ b/tutorial/tracking/render-based/data/sequence1/dragon.0.pos @@ -0,0 +1,6 @@ +-0.005349535388 +-0.01516976067 +0.2050684923 +-2.690336518 +-0.02316506528 +0.06402556549 \ No newline at end of file diff --git a/tutorial/tracking/render-based/data/sequence1/dragon.bam b/tutorial/tracking/render-based/data/sequence1/dragon.bam new file mode 100644 index 0000000000..f6b10b02ce Binary files /dev/null and b/tutorial/tracking/render-based/data/sequence1/dragon.bam differ diff --git a/tutorial/tracking/render-based/data/sequence1/dragon.init b/tutorial/tracking/render-based/data/sequence1/dragon.init new file mode 100644 index 0000000000..77a278c11c --- /dev/null +++ b/tutorial/tracking/render-based/data/sequence1/dragon.init @@ -0,0 +1,5 @@ +4 +-0.0476 0.0276 0.011 # tip of the tongue +0.00435 0.046124 -0.01048 # tip of the crest +0.053011 0.0229 0.002352 # Tip of the tail +0.02405 -0.023972 0.018323 # Leftmost of the back left foot diff --git a/tutorial/tracking/render-based/data/sequence1/dragon.json b/tutorial/tracking/render-based/data/sequence1/dragon.json new file mode 100644 index 0000000000..e6278460af --- /dev/null +++ b/tutorial/tracking/render-based/data/sequence1/dragon.json @@ -0,0 +1,81 @@ +{ + "verbose": { + "enabled": true + }, + "camera": { + "intrinsics": { + "model": "perspectiveWithoutDistortion", + "px": 302.573, + "py": 302.396, + "u0": 162.776, + "v0": 122.475 + }, + "height": 240, + "width": 320 + }, + "vvs": { + "gain": 2.0, + "maxIterations": 10 + }, + "model": "data/sequence1/dragon.obj", + "silhouetteExtractionSettings": { + "threshold": { + "type": "relative", + "value": 0.1 + }, + "sampling": { + "samplingRate": 2, + "numPoints": 128, + "reusePreviousPoints": true + } + }, + "mask": { + "type": "histogram", + "bins": 32, + "objectUpdateRate": 0.1, + "backgroundUpdateRate": 0.1, + "maxDepthError": 0.01 + }, + "drift": { + "type": "probabilistic", + "colorUpdateRate": 0.1, + "initialColorSigma": 15.0, + "depthSigma": 0.01, + "filteringMaxDistance": 0.001, + "minDistanceNewPoints": 0.005 + }, + "features": [ + { + "type": "depth", + "weight": 0.1, + "step": 8, + "useMask": true, + "minMaskConfidence": 0.7 + }, + { + "type": "silhouetteColor", + "weight": 0.01, + "ccd": { + "h": 8, + "delta_h": 1 + } + }, + { + "type": "klt", + "weight": 0.1, + "useMask": true, + "minMaskConfidence": 0.5, + "maxReprojectionErrorPixels": 5.0, + "newPointsMinPixelDistance": 4, + "minimumNumPoints": 20, + "blockSize": 5, + "useHarris": true, + "harris": 0.05, + "maxFeatures": 500, + "minDistance": 5.0, + "pyramidLevels": 3, + "quality": 0.01, + "windowSize": 5 + } + ] +} diff --git a/tutorial/tracking/render-based/data/sequence1/dragon.png b/tutorial/tracking/render-based/data/sequence1/dragon.png new file mode 100644 index 0000000000..06d12c78e5 Binary files /dev/null and b/tutorial/tracking/render-based/data/sequence1/dragon.png differ diff --git a/tutorial/tracking/render-based/render-based-tutorial-utils.h b/tutorial/tracking/render-based/render-based-tutorial-utils.h new file mode 100644 index 0000000000..5bc4db0da2 --- /dev/null +++ b/tutorial/tracking/render-based/render-based-tutorial-utils.h @@ -0,0 +1,401 @@ +#ifndef VP_RB_TRACKER_TUTORIAL_HELPER_H +#define VP_RB_TRACKER_TUTORIAL_HELPER_H + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +#include "pStatClient.h" + +#ifndef DOXYGEN_SHOULD_SKIP_THIS +namespace vpRBTrackerTutorial +{ + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +struct BaseArguments +{ + BaseArguments() : trackerConfiguration(""), maxDepthDisplay(1.f), display(true), debugDisplay(false), enableRenderProfiling(false) { } + + void registerArguments(vpJsonArgumentParser &parser) + { + parser + .addArgument("--tracker", trackerConfiguration, false, "Path to the JSON file containing the tracker") + .addArgument("--object", object, false, "Name of the object to track. Used to potentially fetch the init file") + .addArgument("--init-file", initFile, false, "Path to the JSON file containing the 2D/3D correspondences for initialization by click") + .addArgument("--pose", inlineInit, false, "Initial pose of the object in the camera frame.") + .addArgument("--max-depth-display", maxDepthDisplay, false, "Maximum depth value, used to scale the depth display") + .addFlag("--no-display", display, "Disable display windows") + .addFlag("--debug-display", debugDisplay, "Enable additional displays from the renderer") + .addFlag("--profile", enableRenderProfiling, "Enable the use of Pstats to profile rendering times"); + } + + void postProcessArguments() + { + if (trackerConfiguration.empty()) { + throw vpException(vpException::badValue, "No tracker configuration was specified"); + } + if (object.empty()) { + object = vpIoTools::getName(trackerConfiguration); + object.erase(object.end() - 5, object.end()); + } + if (initFile.empty()) { + initFile = vpIoTools::getParent(trackerConfiguration) + vpIoTools::separator + object + ".init"; + } + + if (!display && inlineInit.empty()) { + throw vpException(vpException::badValue, "Cannot disable displays without specifying the initial pose"); + } + if (inlineInit.size() > 0) { + if (inlineInit.size() != 6) { + throw vpException(vpException::dimensionError, "Inline pose initialization expected to have 6 values (tx, ty, tz, tux, tuy, tuz)"); + } + for (unsigned int i = 0; i < 6; ++i) { + std::cout << "inline i = " << inlineInit[i] << std::endl; + } + cMoInit = vpHomogeneousMatrix(inlineInit[0], inlineInit[1], inlineInit[2], inlineInit[3], inlineInit[4], inlineInit[5]); + } + } + + bool hasInlineInit() + { + return !inlineInit.empty(); + } + + std::string trackerConfiguration; + std::string object; + std::string initFile; + std::vector inlineInit; + float maxDepthDisplay; + vpHomogeneousMatrix cMoInit; + bool display; + bool debugDisplay; + bool enableRenderProfiling; +}; + +class vpRBExperimentLogger +{ +public: + vpRBExperimentLogger() : enabled(false), videoEnabled(false), framerate(30) + { } + + void registerArguments(vpJsonArgumentParser &parser) + { + parser + .addFlag("--save", enabled, "Whether to save experiment data") + .addArgument("--save-path", folder, false, "Where to save the experiment log. The folder should not exist.") + .addFlag("--save-video", videoEnabled, "Whether to save the video") + .addArgument("--video-framerate", framerate, false, "Output video framerate"); + + } + + void startLog() + { + if (enabled) { + if (folder.empty()) { + throw vpException(vpException::badValue, "Experiment logging enabled but folder not specified"); + } + vpIoTools::makeDirectory(folder); + if (videoEnabled) { + videoWriter.setFramerate(framerate); + videoWriter.setCodec(cv::VideoWriter::fourcc('P', 'I', 'M', '1')); + videoWriter.setFileName(folder + vpIoTools::separator + "video.mp4"); + } + } + } + + void logFrame(const vpRBTracker &tracker, unsigned int iter, const vpImage &I, const vpImage &IRGB, const vpImage &Idepth, const vpImage &Imask) + { + if (videoEnabled) { + Iout.resize(IRGB.getHeight() * 2, IRGB.getWidth() * 2); + + vpDisplay::getImage(I, IgrayOverlay); + vpDisplay::getImage(IRGB, IColOverlay); + vpDisplay::getImage(Idepth, IdepthOverlay); + vpDisplay::getImage(Imask, ImaskOverlay); +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel for +#endif + for (unsigned int i = 0; i < IRGB.getHeight(); ++i) { + memcpy(Iout[i], IgrayOverlay[i], IRGB.getWidth() * sizeof(vpRGBa)); + memcpy(Iout[i] + IRGB.getWidth(), IColOverlay[i], IRGB.getWidth() * sizeof(vpRGBa)); + memcpy(Iout[i + IRGB.getHeight()], IdepthOverlay[i], IRGB.getWidth() * sizeof(vpRGBa)); + memcpy(Iout[i + IRGB.getHeight()] + IRGB.getWidth(), ImaskOverlay[i], IRGB.getWidth() * sizeof(vpRGBa)); + } + + if (iter == 1) { + videoWriter.open(Iout); + } + else { + videoWriter.saveFrame(Iout); + } + } + + nlohmann::json iterLog; + vpHomogeneousMatrix cMo; + tracker.getPose(cMo); + iterLog["cMo"] = cMo; + + log.push_back(iterLog); + } + + void close() + { + if (videoEnabled) { + videoWriter.close(); + } + std::ofstream f(folder + vpIoTools::separator + "log.json"); + f << log.dump(2) << std::endl; + f.close(); + } + +private: + bool enabled; + std::string folder; + + vpImage IColOverlay; + vpImage IgrayOverlay; + vpImage IdepthOverlay; + vpImage ImaskOverlay; + vpImage Iout; + + bool videoEnabled; + unsigned int framerate; + vpVideoWriter videoWriter; + + nlohmann::json log; +}; + +class vpRBExperimentPlotter +{ +public: + + vpRBExperimentPlotter() : enabled(false), plotPose(false), plotPose3d(false), plotDivergenceMetrics(false), plotCovariance(false) { } + + void registerArguments(vpJsonArgumentParser &parser) + { + parser + .addFlag("--plot-pose", plotPose, "Plot the pose of the object in the camera frame") + .addFlag("--plot-position", plotPose3d, "Plot the position of the object in a 3d figure") + .addFlag("--plot-divergence", plotDivergenceMetrics, "Plot the metrics associated to the divergence threshold computation") + .addFlag("--plot-cov", plotCovariance, "Plot the pose covariance trace for each feature"); + + } + + void postProcessArguments(bool displayEnabled) + { + enabled = plotPose || plotDivergenceMetrics || plotPose3d || plotCovariance; + if (enabled && !displayEnabled) { + throw vpException(vpException::badValue, "Tried to plot data, but display is disabled"); + } + } + + void init(std::vector> &displays) + { + if (!enabled) { + return; + } + int ypos = 0, xpos = 0; + for (std::shared_ptr &display : displays) { + ypos = std::min(ypos, display->getWindowYPosition()); + xpos = std::max(xpos, display->getWindowXPosition() + static_cast(display->getWidth())); + } + + numPlots = static_cast(plotPose) + static_cast(plotDivergenceMetrics) + static_cast(plotPose3d) + static_cast(plotCovariance); + plotter.init(numPlots, 600, 800, xpos, ypos, "Plot"); + unsigned int plotIndex = 0; + if (plotPose) { + plotter.initGraph(plotIndex, 6); + plotter.setTitle(plotIndex, "cMo"); + std::vector legends = { + "tx", "ty", "tz", "tux", "tuy", "tuz" + }; + for (unsigned int i = 0; i < 6; ++i) { + plotter.setLegend(plotIndex, i, legends[i]); + } + plotter.setGraphThickness(plotIndex, 2); + ++plotIndex; + } + if (plotPose3d) { + plotter.initGraph(plotIndex, 1); + plotter.setTitle(plotIndex, "3D object position"); + plotter.setGraphThickness(plotIndex, 2); + ++plotIndex; + } + + if (plotDivergenceMetrics) { + plotter.initGraph(plotIndex, 1); + plotter.initRange(plotIndex, 0.0, 1.0, 0.0, 1.0); + plotter.setTitle(plotIndex, "Divergence"); + ++plotIndex; + } + if (plotCovariance) { + plotter.initGraph(plotIndex, 2); + plotter.setLegend(plotIndex, 0, "Translation trace standard deviation (cm)"); + plotter.setLegend(plotIndex, 1, "Rotation trace standard deviation (deg)"); + + plotter.setTitle(plotIndex, "Covariance"); + ++plotIndex; + } + } + + void plot(const vpRBTracker &tracker, double time) + { + if (!enabled) { + return; + } + unsigned int plotIndex = 0; + if (plotPose) { + vpHomogeneousMatrix cMo; + tracker.getPose(cMo); + plotter.plot(plotIndex, time, vpPoseVector(cMo)); + ++plotIndex; + } + if (plotPose3d) { + vpHomogeneousMatrix cMo; + tracker.getPose(cMo); + vpTranslationVector t = cMo.getTranslationVector(); + plotter.plot(plotIndex, 0, t[0], t[1], t[2]); + ++plotIndex; + } + if (plotDivergenceMetrics) { + const std::shared_ptr driftDetector = tracker.getDriftDetector(); + double metric = driftDetector ? driftDetector->getScore() : 0.0; + plotter.plot(plotIndex, 0, time, metric); + ++plotIndex; + } + if (plotCovariance) { + vpMatrix cov = tracker.getCovariance(); + double traceTranslation = 0.0, traceRotation = 0.0; + for (unsigned int i = 0; i < 3; ++i) { + traceTranslation += cov[i][i]; + traceRotation += cov[i + 3][i + 3]; + } + traceTranslation = sqrt(traceTranslation) * 100; + traceRotation = vpMath::deg(sqrt(traceRotation)); + + plotter.plot(plotIndex, 0, time, traceTranslation); + plotter.plot(plotIndex, 1, time, traceRotation); + + ++plotIndex; + } + } +private: + bool enabled; + bool plotPose; + bool plotPose3d; + bool plotDivergenceMetrics; + bool plotCovariance; + int numPlots; + vpPlot plotter; +}; + +std::vector> createDisplays( + vpImage &Id, vpImage &Icol, + vpImage &depthDisplay, vpImage &probaDisplay) +{ + return vpDisplayFactory::makeDisplayGrid( + 2, 2, + 0, 0, + 20, 40, + "Grayscale", Id, + "Color", Icol, + "Depth", depthDisplay, + "Proba mask", probaDisplay + ); +} + +std::vector> createDisplays( + vpImage &Id, vpImage &Icol, vpImage &probaDisplay) +{ + return vpDisplayFactory::makeDisplayGrid( + 1, 3, + 0, 0, + 20, 40, + "Grayscale", Id, + "Color", Icol, + "Proba mask", probaDisplay + ); +} + +void enableRendererProfiling() +{ + if (PStatClient::is_connected()) { + PStatClient::disconnect(); + } + + std::string host = ""; // Empty = default config var value + int port = -1; // -1 = default config var value + if (!PStatClient::connect(host, port)) { + std::cout << "Could not connect to PStat server." << std::endl; + } +} + +void displayNormals(const vpImage &normalsImage, vpImage &normalDisplayImage) +{ +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel for +#endif + for (unsigned 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 displayCanny(const vpImage &cannyRawData, + vpImage &canny, const vpImage &valid) +{ +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel for +#endif + for (unsigned int i = 0; i < cannyRawData.getSize(); ++i) { + //vpRGBf &px = cannyRawData.bitmap[i]; + canny.bitmap[i] = valid.bitmap[i] * 255; + //canny.bitmap[i] = static_cast(127.5f + 127.5f * atan(px.B)); + } + + vpDisplay::display(canny); + for (unsigned int i = 0; i < canny.getHeight(); i += 4) { + for (unsigned int j = 0; j < canny.getWidth(); j += 4) { + if (!valid[i][j]) 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); +} +} +#endif +#endif diff --git a/tutorial/tracking/render-based/tutorial-rbt-realsense.cpp b/tutorial/tracking/render-based/tutorial-rbt-realsense.cpp new file mode 100644 index 0000000000..7b3570286d --- /dev/null +++ b/tutorial/tracking/render-based/tutorial-rbt-realsense.cpp @@ -0,0 +1,242 @@ +//! \example tutorial-rbt-realsense.cpp +#include +#include + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +#ifndef VISP_HAVE_REALSENSE2 + +int main() +{ + std::cerr << "To run this tutorial, recompile ViSP with the Realsense third party library" << std::endl; + return EXIT_SUCCESS; +} + +#else +#include +#include + +#include + +#include "render-based-tutorial-utils.h" + +#ifndef DOXYGEN_SHOULD_SKIP_THIS +struct CmdArguments +{ + CmdArguments() : height(480), width(848), fps(60) + { } + + void registerArguments(vpJsonArgumentParser &parser) + { + parser.addArgument("--height", height, false, "Realsense requested image height") + .addArgument("--width", width, false, "Realsense requested image width") + .addArgument("--fps", fps, false, "Realsense requested framerate"); + } + + unsigned int height, width, fps; +}; +#endif + +void updateDepth(const vpImage &depthRaw, float depthScale, float maxZDisplay, vpImage &depth, vpImage &IdepthDisplay) +{ + depth.resize(depthRaw.getHeight(), depthRaw.getWidth()); +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel for +#endif + for (unsigned int i = 0; i < depthRaw.getSize(); ++i) { + depth.bitmap[i] = depthScale * static_cast(depthRaw.bitmap[i]); + IdepthDisplay.bitmap[i] = depth.bitmap[i] > maxZDisplay ? 0 : static_cast((depth.bitmap[i] / maxZDisplay) * 255.f); + } +} + +int main(int argc, const char **argv) +{ + // Read the command line options + vpRBTrackerTutorial::BaseArguments baseArgs; + CmdArguments realsenseArgs; + vpRBTrackerTutorial::vpRBExperimentLogger logger; + vpRBTrackerTutorial::vpRBExperimentPlotter plotter; + + vpJsonArgumentParser parser( + "Tutorial showing the usage of the Render-Based tracker with a RealSense camera", + "--config", "/" + ); + + baseArgs.registerArguments(parser); + realsenseArgs.registerArguments(parser); + logger.registerArguments(parser); + plotter.registerArguments(parser); + + parser.parse(argc, argv); + + baseArgs.postProcessArguments(); + plotter.postProcessArguments(baseArgs.display); + + if (baseArgs.enableRenderProfiling) { + vpRBTrackerTutorial::enableRendererProfiling(); + } + + std::cout << "Loading tracker: " << baseArgs.trackerConfiguration << std::endl; + vpRBTracker tracker; + tracker.loadConfigurationFile(baseArgs.trackerConfiguration); + tracker.startTracking(); + const unsigned int width = realsenseArgs.width, height = realsenseArgs.height; + const unsigned fps = realsenseArgs.fps; + + vpImage Id(height, width); + vpImage Icol(height, width); + vpImage depthRaw(height, width); + vpImage depth(height, width); + vpImage IdepthDisplay(height, width); + vpImage IProbaDisplay(height, width); + vpImage cannyDisplay(height, width); + vpImage InormDisplay(height, width); + + vpRealSense2 realsense; + + std::cout << "Opening realsense with " << width << "x" << height << " @ " << fps << "fps" << std::endl; + rs2::config config; + config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps); + config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps); + rs2::align align_to(RS2_STREAM_COLOR); + try { + realsense.open(config); + } + catch (const vpException &e) { + std::cout << "Caught an exception: " << e.what() << std::endl; + std::cout << "Check if the Realsense camera is connected..." << std::endl; + return EXIT_SUCCESS; + } + + float depthScale = realsense.getDepthScale(); + //camera warmup + for (int i = 0; i < 10; ++i) { + realsense.acquire(Icol); + } + vpImageConvert::convert(Icol, Id); + + vpCameraParameters cam = realsense.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithoutDistortion); + tracker.setCameraParameters(cam, height, width); + + std::cout << "Creating displays" << std::endl;; + std::vector> displays, displaysDebug; + + if (baseArgs.display) { + displays = vpRBTrackerTutorial::createDisplays(Id, Icol, IdepthDisplay, IProbaDisplay); + if (baseArgs.debugDisplay) { + displaysDebug = vpDisplayFactory::makeDisplayGrid(1, 2, + 0, 0, + 20, 20, + "Normals in object frame", InormDisplay, + "Depth canny", cannyDisplay + ); + } + plotter.init(displays); + } + + if (baseArgs.display && !baseArgs.hasInlineInit()) { + bool ready = false; + while (!ready) { + realsense.acquire((unsigned char *)Icol.bitmap, (unsigned char *)depthRaw.bitmap, nullptr, nullptr, &align_to); + updateDepth(depthRaw, depthScale, baseArgs.maxDepthDisplay, depth, IdepthDisplay); + vpImageConvert::convert(Icol, Id); + vpDisplay::display(Icol); vpDisplay::display(Id); vpDisplay::display(IdepthDisplay); + vpDisplay::flush(Icol); vpDisplay::flush(Id); vpDisplay::flush(IdepthDisplay); + if (vpDisplay::getClick(Id, false)) { + ready = true; + } + else { + vpTime::wait(1000.0 / fps); + } + } + } + + updateDepth(depthRaw, depthScale, baseArgs.maxDepthDisplay, depth, IdepthDisplay); + + vpHomogeneousMatrix cMo; + + // Manual initialization of the tracker + std::cout << "Starting init" << std::endl; + if (baseArgs.hasInlineInit()) { + tracker.setPose(baseArgs.cMoInit); + } + else if (baseArgs.display) { + + tracker.initClick(Id, baseArgs.initFile, true); + tracker.getPose(cMo); + } + else { + throw vpException(vpException::notImplementedError, "Cannot initalize tracking: no auto init function provided"); + } + + std::cout << "Starting pose: " << vpPoseVector(cMo).t() << std::endl; + + if (baseArgs.display) { + vpDisplay::flush(Id); + } + +//vpRBTrackerFilter &ukfm = tracker.getFilter(); + logger.startLog(); + unsigned int iter = 1; + // Main tracking loop + double expStart = vpTime::measureTimeMs(); + while (true) { + double frameStart = vpTime::measureTimeMs(); + // Acquire images + realsense.acquire((unsigned char *)Icol.bitmap, (unsigned char *)depthRaw.bitmap, nullptr, nullptr, &align_to); + updateDepth(depthRaw, depthScale, baseArgs.maxDepthDisplay, depth, IdepthDisplay); + vpImageConvert::convert(Icol, Id); + + // Pose tracking + double trackingStart = vpTime::measureTimeMs(); + tracker.track(Id, Icol, depth); + double trackingEnd = vpTime::measureTimeMs(); + tracker.getPose(cMo); + double displayStart = vpTime::measureTimeMs(); + if (baseArgs.display) { + if (baseArgs.debugDisplay) { + const vpRBFeatureTrackerInput &lastFrame = tracker.getMostRecentFrame(); + + vpRBTrackerTutorial::displayCanny(lastFrame.renders.silhouetteCanny, cannyDisplay, lastFrame.renders.isSilhouette); + } + + vpDisplay::display(IdepthDisplay); + vpDisplay::display(Id); + // vpDisplay::display(Icol); + tracker.display(Id, Icol, IdepthDisplay); + vpDisplay::displayFrame(Icol, cMo, cam, 0.05, vpColor::none, 2); + vpDisplay::displayText(Id, 20, 5, "Right click to exit", vpColor::red); + vpMouseButton::vpMouseButtonType button; + if (vpDisplay::getClick(Id, button, false)) { + if (button == vpMouseButton::button3) { + break; + } + } + tracker.displayMask(IProbaDisplay); + vpDisplay::display(IProbaDisplay); + + vpDisplay::flush(Id); vpDisplay::flush(Icol); + vpDisplay::flush(IdepthDisplay); vpDisplay::flush(IProbaDisplay); + } + + logger.logFrame(tracker, iter, Id, Icol, IdepthDisplay, IProbaDisplay); + const double displayEnd = vpTime::measureTimeMs(); + + // ukfm.filter(cMo, 0.05); + // const vpHomogeneousMatrix cMoFiltered = ukfm.getFilteredPose(); + // vpDisplay::displayFrame(Icol, cMoFiltered, cam, 0.05, vpColor::yellow, 2); + + const double frameEnd = vpTime::measureTimeMs(); + std::cout << "Iter " << iter << ": " << round(frameEnd - frameStart) << "ms" << std::endl; + std::cout << "- Tracking: " << round(trackingEnd - trackingStart) << "ms" << std::endl; + std::cout << "- Display: " << round(displayEnd - displayStart) << "ms" << std::endl; + plotter.plot(tracker, (frameEnd - expStart) / 1000.0); + iter++; + } + + logger.close(); + return EXIT_SUCCESS; +} +#endif diff --git a/tutorial/tracking/render-based/tutorial-rbt-sequence.cpp b/tutorial/tracking/render-based/tutorial-rbt-sequence.cpp new file mode 100644 index 0000000000..71619676c0 --- /dev/null +++ b/tutorial/tracking/render-based/tutorial-rbt-sequence.cpp @@ -0,0 +1,244 @@ +//! \example tutorial-rbt-sequence.cpp +#include +#include +#include +#include + +#include +#include + +#include + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + +#include "render-based-tutorial-utils.h" + +struct CmdArguments +{ + CmdArguments() : startFrame(0), frameStep(1), stepByStep(false) + { + + } + + void registerArguments(vpJsonArgumentParser &parser) + { + parser + .addArgument("--color", colorSequence, true, "The color sequence (in video reader format, eg., /path/to/I\%04d.png)") + .addArgument("--depth", depthFolder, false, "The depth images associated to the color sequence. Frames should be aligned") + .addArgument("--start", startFrame, false, "The first frame of the sequence") + .addArgument("--step", frameStep, false, "How many frames should be read between calls to the tracker") + .addFlag("--step-by-step", stepByStep, "Go through the sequence interactively, frame by frame"); + } + + void postProcessArguments() + { + if (colorSequence.empty()) { + throw vpException(vpException::badValue, "Input sequence should not be empty"); + } + } + + std::string colorSequence; + std::string depthFolder; + unsigned int startFrame; + unsigned int frameStep; + bool stepByStep; +}; + +int main(int argc, const char **argv) +{ + vpRBTrackerTutorial::BaseArguments baseArgs; + CmdArguments sequenceArgs; + vpRBTrackerTutorial::vpRBExperimentLogger logger; + vpRBTrackerTutorial::vpRBExperimentPlotter plotter; + + vpJsonArgumentParser parser("Tutorial showing how to use the Render-Based Tracker on an offline sequence", "--config", "/"); + baseArgs.registerArguments(parser); + sequenceArgs.registerArguments(parser); + logger.registerArguments(parser); + plotter.registerArguments(parser); + + parser.parse(argc, argv); + + baseArgs.postProcessArguments(); + sequenceArgs.postProcessArguments(); + plotter.postProcessArguments(baseArgs.display); + + if (baseArgs.enableRenderProfiling) { + vpRBTrackerTutorial::enableRendererProfiling(); + } + + baseArgs.display = true; + // Get the option values + + logger.startLog(); + + // Set tracking and rendering parameters + vpCameraParameters cam; + + vpRBTracker tracker; + tracker.loadConfigurationFile(baseArgs.trackerConfiguration); + tracker.startTracking(); + cam = tracker.getCameraParameters(); + + //VideoReader to read images from disk + + vpImage Icol; + vpVideoReader readerRGB; + readerRGB.setFileName(sequenceArgs.colorSequence); + readerRGB.setFirstFrameIndex(sequenceArgs.startFrame); + readerRGB.open(Icol); + readerRGB.acquire(Icol); + + const int width = readerRGB.getWidth(); + const int height = readerRGB.getHeight(); + + vpImage Id(height, width); + vpImage depth(height, width); + vpImage depthDisplay(height, width); + vpImage IProba(height, width); + vpImage IProbaDisplay(height, width); + vpImage IRender(height, width); + vpImage InormDisplay(height, width); + vpImage ICannyDisplay(height, width); + + vpImageConvert::convert(Icol, Id); + + // Main window creation and displaying + + std::vector> displays, debugDisplays; + + if (baseArgs.display) { + displays = vpRBTrackerTutorial::createDisplays(Id, Icol, depthDisplay, IProbaDisplay); + if (baseArgs.debugDisplay) { + debugDisplays = vpDisplayFactory::makeDisplayGrid( + 1, 3, + 0, 0, + 20, 20, + "Normals in object frame", InormDisplay, + "Depth canny", ICannyDisplay, + "Color render", IRender + ); + } + plotter.init(displays); + } + + vpHomogeneousMatrix cMo; + + nlohmann::json result = nlohmann::json::array(); + + // Manual initialization of the tracker + std::cout << "Starting init" << std::endl; + + if (baseArgs.hasInlineInit()) { + tracker.setPose(baseArgs.cMoInit); + } + else if (baseArgs.display) { + tracker.initClick(Id, baseArgs.initFile, true); + } + else { + throw vpException(vpException::notImplementedError, "Cannot initialize tracking: no auto init function provided"); + } + + if (baseArgs.display) { + vpDisplay::flush(Id); + } + + int im = sequenceArgs.startFrame; + unsigned int iter = 1; + // Main tracking loop + double expStart = vpTime::measureTimeMs(); + + while (true) { + double frameStart = vpTime::measureTimeMs(); + // Acquire images + for (unsigned int sp = 0; sp < sequenceArgs.frameStep; ++sp) { + + readerRGB.acquire(Icol); + vpImageConvert::convert(Icol, Id); + if (!sequenceArgs.depthFolder.empty()) { + std::stringstream depthName; + depthName << sequenceArgs.depthFolder << "/" << std::setfill('0') << std::setw(6) << im << ".npy"; + visp::cnpy::NpyArray npz_data = visp::cnpy::npy_load(depthName.str()); + vpImage dataArray(npz_data.data(), npz_data.shape[0], npz_data.shape[1], false); + float scale = 9.999999747378752e-05; + depth.resize(dataArray.getHeight(), dataArray.getWidth()); + depthDisplay.resize(dataArray.getHeight(), dataArray.getWidth()); +#ifdef VISP_HAVE_OPENMP +#pragma omp parallel for +#endif + for (unsigned int i = 0; i < dataArray.getSize(); ++i) { + float value = static_cast(dataArray.bitmap[i]) * scale; + depth.bitmap[i] = value; + depthDisplay.bitmap[i] = value > baseArgs.maxDepthDisplay ? 0.f : static_cast((depth.bitmap[i] / baseArgs.maxDepthDisplay) * 255.f); + } + } + } + + // Pose tracking + double trackingStart = vpTime::measureTimeMs(); + if (depth.getSize() == 0) { + tracker.track(Id, Icol); + } + else { + tracker.track(Id, Icol, depth); + } + std::cout << "Tracking took " << vpTime::measureTimeMs() - trackingStart << "ms" << std::endl; + + if (baseArgs.display) { + if (baseArgs.debugDisplay) { + const vpRBFeatureTrackerInput &lastFrame = tracker.getMostRecentFrame(); + + vpRBTrackerTutorial::displayNormals(lastFrame.renders.normals, InormDisplay); + + vpRBTrackerTutorial::displayCanny(lastFrame.renders.silhouetteCanny, ICannyDisplay, lastFrame.renders.isSilhouette); + if (lastFrame.renders.color.getSize() > 0) { + IRender = lastFrame.renders.color; + vpDisplay::display(IRender); + vpDisplay::flush(IRender); + } + } + + tracker.displayMask(IProbaDisplay); + vpDisplay::display(IProbaDisplay); + vpDisplay::flush(IProbaDisplay); + vpDisplay::display(Id); + // vpDisplay::display(Icol); + tracker.display(Id, Icol, depthDisplay); + vpDisplay::displayFrame(Icol, cMo, cam, 0.05, vpColor::none, 2); + + vpDisplay::flush(Icol); + vpDisplay::flush(Id); + if (depth.getSize() > 0) { + vpDisplay::display(depthDisplay); + vpDisplay::flush(depthDisplay); + } + } + + tracker.getPose(cMo); + result.push_back(cMo); + + logger.logFrame(tracker, iter, Id, Icol, depthDisplay, IProbaDisplay); + + if (sequenceArgs.stepByStep && baseArgs.display) { + vpDisplay::getClick(Id, true); + } + + std::cout << "Iter: " << iter << std::endl; + ++im; + ++iter; + if (im > readerRGB.getLastFrameIndex()) { + break; + } + + double frameEnd = vpTime::measureTimeMs(); + std::cout << "Frame took: " << frameEnd - frameStart << "ms" << std::endl; + plotter.plot(tracker, (frameEnd - expStart) / 1000.0); + + } + + logger.close(); + + return EXIT_SUCCESS; +}