Visual Servoing Platform  version 3.6.1 under development (2025-03-06)
vpPanda3DGeometryRenderer.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  */
30 
31 #include <visp3/core/vpConfig.h>
32 
33 #if defined(VISP_HAVE_PANDA3D)
34 
35 #include <visp3/ar/vpPanda3DGeometryRenderer.h>
36 #include "windowFramework.h"
37 #include "graphicsOutput.h"
38 #include "graphicsEngine.h"
39 #include "graphicsBuffer.h"
40 
41 BEGIN_VISP_NAMESPACE
42 
43 const std::string SHADER_VERT_NORMAL_AND_DEPTH_CAMERA =
44 "#version 330\n"
45 "in vec3 p3d_Normal;\n"
46 "in vec4 p3d_Vertex;\n"
47 "uniform mat3 p3d_NormalMatrix;\n"
48 "uniform mat4 p3d_ModelViewMatrix;\n"
49 "uniform mat4 p3d_ModelViewProjectionMatrix;\n"
50 "out vec3 oNormal;\n"
51 "out float distToCamera;\n"
52 "void main()\n"
53 "{\n"
54 " gl_Position = p3d_ModelViewProjectionMatrix * p3d_Vertex;\n"
55 " oNormal = p3d_NormalMatrix * normalize(p3d_Normal);\n"
56 " vec4 cs_position = p3d_ModelViewMatrix * p3d_Vertex;\n"
57 " distToCamera = -cs_position.z;\n"
58 "}\n";
59 
60 const std::string SHADER_VERT_NORMAL_AND_DEPTH_OBJECT =
61 "#version 330\n"
62 "in vec3 p3d_Normal;\n"
63 "in vec4 p3d_Vertex;\n"
64 "uniform mat4 p3d_ModelViewMatrix;\n"
65 "uniform mat4 p3d_ModelViewProjectionMatrix;\n"
66 "out vec3 oNormal;\n"
67 "out float distToCamera;\n"
68 "void main()\n"
69 "{\n"
70 " gl_Position = p3d_ModelViewProjectionMatrix * p3d_Vertex;\n"
71 " oNormal = vec3(p3d_Normal.x, -p3d_Normal.z, p3d_Normal.y);\n"
72 " vec4 cs_position = p3d_ModelViewMatrix * p3d_Vertex;\n"
73 " distToCamera = -cs_position.z;\n"
74 "}\n";
75 
76 const std::string SHADER_FRAG_NORMAL_AND_DEPTH =
77 "#version 330\n"
78 "in vec3 oNormal;\n"
79 "in float distToCamera;\n"
80 "out vec4 p3d_FragData;\n"
81 "void main()\n"
82 "{\n"
83 " p3d_FragData.bgra = vec4(normalize(oNormal), distToCamera);\n"
84 "}\n";
85 
86 std::string renderTypeToName(vpPanda3DGeometryRenderer::vpRenderType type)
87 {
88  switch (type) {
89  case vpPanda3DGeometryRenderer::vpRenderType::OBJECT_NORMALS:
90  return "normals-world";
91  case vpPanda3DGeometryRenderer::vpRenderType::CAMERA_NORMALS:
92  return "normals-camera";
93  default:
94  return "";
95  }
96 }
97 
98 vpPanda3DGeometryRenderer::vpPanda3DGeometryRenderer(vpRenderType renderType) : vpPanda3DBaseRenderer(renderTypeToName(renderType)), m_renderType(renderType) { }
99 
101 {
102  m_renderRoot = m_window->get_render().attach_new_node(m_name);
103  PT(Shader) shader;
104  if (m_renderType == OBJECT_NORMALS) {
105  shader = Shader::make(Shader::ShaderLanguage::SL_GLSL,
106  SHADER_VERT_NORMAL_AND_DEPTH_OBJECT,
107  SHADER_FRAG_NORMAL_AND_DEPTH);
108  }
109  else if (m_renderType == CAMERA_NORMALS) {
110  shader = Shader::make(Shader::ShaderLanguage::SL_GLSL,
111  SHADER_VERT_NORMAL_AND_DEPTH_CAMERA,
112  SHADER_FRAG_NORMAL_AND_DEPTH);
113  }
114  m_renderRoot.set_shader(shader);
115 }
116 
118 {
119  if (m_window == nullptr) {
120  throw vpException(vpException::fatalError, "Cannot setup render target when window is null");
121  }
122  FrameBufferProperties fbp;
123  fbp.set_rgb_color(true);
124  fbp.set_float_depth(false);
125  fbp.set_float_color(true);
126  fbp.set_depth_bits(16);
127  fbp.set_rgba_bits(32, 32, 32, 32);
128 
129  WindowProperties win_prop;
131  // Don't open a window - force it to be an offscreen buffer.
132  int flags = GraphicsPipe::BF_refuse_window | GraphicsPipe::BF_resizeable;
133  GraphicsOutput *windowOutput = m_window->get_graphics_output();
134  GraphicsEngine *engine = windowOutput->get_engine();
135  GraphicsPipe *pipe = windowOutput->get_pipe();
136 
137  static int id = 0;
138  m_normalDepthBuffer = engine->make_output(pipe, renderTypeToName(m_renderType) + std::to_string(id), m_renderOrder, fbp, win_prop, flags,
139  windowOutput->get_gsg(), windowOutput);
140  m_normalDepthTexture = new Texture("geometry texture " + std::to_string(id));
141  ++id;
142  if (m_normalDepthBuffer == nullptr) {
143  throw vpException(vpException::fatalError, "Could not create geometry info buffer");
144  }
145  // if (!m_normalDepthBuffer->is_valid()) {
146  // throw vpException(vpException::fatalError, "Geometry info buffer is invalid");
147  // }
148  m_buffers.push_back(m_normalDepthBuffer);
149  m_normalDepthBuffer->set_inverted(windowOutput->get_gsg()->get_copy_texture_inverted());
150  fbp.setup_color_texture(m_normalDepthTexture);
151  m_normalDepthTexture->set_format(Texture::F_rgba32);
152  m_normalDepthBuffer->add_render_texture(m_normalDepthTexture, GraphicsOutput::RenderTextureMode::RTM_copy_texture, GraphicsOutput::RenderTexturePlane::RTP_color);
153  m_normalDepthBuffer->set_clear_color(LColor(0.f));
154  m_normalDepthBuffer->set_clear_color_active(true);
155 
156  DisplayRegion *region = m_normalDepthBuffer->make_display_region();
157  if (region == nullptr) {
158  throw vpException(vpException::fatalError, "Could not create display region");
159  }
160  region->set_camera(m_cameraPath);
161  region->set_clear_color(LColor(0.f));
162 }
163 
165 {
166  normals.resize(m_normalDepthTexture->get_y_size(), m_normalDepthTexture->get_x_size());
167  depth.resize(m_normalDepthTexture->get_y_size(), m_normalDepthTexture->get_x_size());
168  if (m_normalDepthTexture->get_component_type() != Texture::T_float) {
169  throw vpException(vpException::badValue, "Unexpected data type in normals texture");
170  }
171 
172  int rowIncrement = normals.getWidth() * 4;
173  float *data = (float *)(&(m_normalDepthTexture->get_ram_image().front()));
174  data = data + rowIncrement * (normals.getHeight() - 1);
175  rowIncrement = -rowIncrement;
176 
177  for (unsigned int i = 0; i < normals.getHeight(); ++i) {
178  vpRGBf *normalRow = normals[i];
179  float *depthRow = depth[i];
180  for (unsigned int j = 0; j < normals.getWidth(); ++j) {
181  normalRow[j].R = (data[j * 4]);
182  normalRow[j].G = (data[j * 4 + 1]);
183  normalRow[j].B = (data[j * 4 + 2]);
184  depthRow[j] = (data[j * 4 + 3]);
185  }
186  data += rowIncrement;
187  }
188 }
189 
190 void vpPanda3DGeometryRenderer::getRender(vpImage<vpRGBf> &normals, vpImage<float> &depth, const vpRect &bb, unsigned int h, unsigned w) const
191 {
192  normals.resize(h, w);
193  // memset(normals.bitmap, 0, normals.getSize() * sizeof(vpRGBf));
194  depth.resize(normals.getHeight(), normals.getWidth(), 0.f);
195  // memset(depth.bitmap, 0, normals.getSize());
196 
197  const unsigned top = static_cast<unsigned int>(std::max(0.0, bb.getTop()));
198  const unsigned left = static_cast<unsigned int>(std::max(0.0, bb.getLeft()));
199  const unsigned numComponents = m_normalDepthTexture->get_num_components();
200  const unsigned rowIncrement = m_renderParameters.getImageWidth() * numComponents; // we ask for only 8 bits image, but we may get an rgb image
201  const float *data = (float *)(&(m_normalDepthTexture->get_ram_image().front()));
202  // Panda3D stores data upside down
203  data += rowIncrement * (m_renderParameters.getImageHeight() - 1);
204  if (numComponents != 4) {
205  throw vpException(vpException::dimensionError, "Expected panda texture to have 4 components!");
206  }
207  if (m_normalDepthTexture->get_component_type() != Texture::T_float) {
208  throw vpException(vpException::badValue, "Unexpected data type in normals texture");
209  }
210 
211  int image_width = static_cast<int>(m_renderParameters.getImageWidth());
212  for (unsigned int i = 0; i < m_renderParameters.getImageHeight(); ++i) {
213  const float *const rowData = data - i * rowIncrement;
214  vpRGBf *normalRow = normals[top + i];
215  float *depthRow = depth[top + i];
216 #if defined(VISP_HAVE_OPENMP)
217 #pragma omp parallel for
218 #endif
219  for (int j = 0; j < image_width; ++j) {
220  int left_j = left + j;
221  int j_4 = j * 4;
222  normalRow[left_j].R = (rowData[j_4]);
223  normalRow[left_j].G = (rowData[j_4 + 1]);
224  normalRow[left_j].B = (rowData[j_4 + 2]);
225  depthRow[left_j] = (rowData[j_4 + 3]);
226  }
227  }
228 }
229 
231 {
232  normals.resize(m_normalDepthTexture->get_y_size(), m_normalDepthTexture->get_x_size());
233  if (m_normalDepthTexture->get_component_type() != Texture::T_float) {
234  throw vpException(vpException::badValue, "Unexpected data type in normals texture");
235  }
236 
237  int rowIncrement = normals.getWidth() * 4;
238  float *data = (float *)(&(m_normalDepthTexture->get_ram_image().front()));
239  data = data + rowIncrement * (normals.getHeight() - 1);
240  rowIncrement = -rowIncrement;
241  for (unsigned int i = 0; i < normals.getHeight(); ++i) {
242  vpRGBf *normalRow = normals[i];
243  for (unsigned int j = 0; j < normals.getWidth(); ++j) {
244  normalRow[j].R = (data[j * 4]);
245  normalRow[j].G = (data[j * 4 + 1]);
246  normalRow[j].B = (data[j * 4 + 2]);
247  }
248  data += rowIncrement;
249  }
250 }
251 
253 {
254  depth.resize(m_normalDepthTexture->get_y_size(), m_normalDepthTexture->get_x_size());
255 
256  if (m_normalDepthTexture->get_component_type() != Texture::T_float) {
257  throw vpException(vpException::badValue, "Unexpected data type in normals texture");
258  }
259 
260  int rowIncrement = depth.getWidth() * 4;
261  float *data = (float *)(&(m_normalDepthTexture->get_ram_image().front()));
262  data = data + rowIncrement * (depth.getHeight() - 1);
263  rowIncrement = -rowIncrement;
264 
265  for (unsigned int i = 0; i < depth.getHeight(); ++i) {
266  float *depthRow = depth[i];
267  for (unsigned int j = 0; j < depth.getWidth(); ++j) {
268  depthRow[j] = (data[j * 4 + 3]);
269  }
270  data += rowIncrement;
271  }
272 }
273 
274 END_VISP_NAMESPACE
275 
276 #elif !defined(VISP_BUILD_SHARED_LIBS)
277 // Work around to avoid warning: libvisp_ar.a(vpPanda3DGeometryRenderer.cpp.o) has no symbols
278 void dummy_vpPanda3DGeometryRenderer() { };
279 
280 #endif
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ fatalError
Fatal error.
Definition: vpException.h:72
unsigned int getWidth() const
Definition: vpImage.h:242
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:544
unsigned int getHeight() const
Definition: vpImage.h:181
Base class for a panda3D renderer. This class handles basic functionalities, such as loading object,...
NodePath m_renderRoot
Rendering parameters.
std::string m_name
Inverse of VISP_T_PANDA.
std::vector< PointerTo< GraphicsOutput > > m_buffers
NodePath of the camera.
PointerTo< WindowFramework > m_window
Rendering priority for this renderer and its buffers. A lower value will be rendered first....
int m_renderOrder
name of the renderer
vpPanda3DRenderParameters m_renderParameters
Pointer to owning window, which can create buffers etc. It is not necessarily visible.
void getRender(vpImage< vpRGBf > &colorData, vpImage< float > &depth) const
Get render results into ViSP readable structures.
void setupScene() VP_OVERRIDE
Initialize the scene for this specific renderer.
void setupRenderTarget() VP_OVERRIDE
Initialize buffers and other objects that are required to save the render.
vpPanda3DGeometryRenderer(vpRenderType renderType)
@ CAMERA_NORMALS
Surface normals in the object frame.
Definition: vpRGBf.h:64
float B
Blue component.
Definition: vpRGBf.h:157
float G
Green component.
Definition: vpRGBf.h:156
float R
Red component.
Definition: vpRGBf.h:155
Defines a rectangle in the plane.
Definition: vpRect.h:79
double getLeft() const
Definition: vpRect.h:173
double getTop() const
Definition: vpRect.h:192