moveit2
The MoveIt Motion Planning Framework for ROS 2.
stereo_camera_model.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Suat Gedikli */
36 
39 
41  float near_clipping_plane_distance,
42  float far_clipping_plane_distance, float fx, float fy, float cx,
43  float cy, float base_line, float disparity_resolution)
44  : SensorModel::Parameters(width, height, near_clipping_plane_distance, far_clipping_plane_distance)
45  , fx_(fx)
46  , fy_(fy)
47  , cx_(cx)
48  , cy_(cy)
49  , base_line_(base_line)
50  , disparity_resolution_(disparity_resolution)
51  , padding_coefficients_(Eigen::Vector3f(disparity_resolution_ / (fx_ * base_line_), 0, 0))
52 {
53 }
54 
56 
58 {
59  return new StereoCameraModel::Parameters(width_, height_, near_clipping_plane_distance_, far_clipping_plane_distance_,
60  fx_, fy_, cx_, cy_, base_line_, disparity_resolution_);
61 }
62 
63 void mesh_filter::StereoCameraModel::Parameters::setCameraParameters(float fx, float fy, float cx, float cy)
64 {
65  fx_ = fx;
66  fy_ = fy;
67  cx_ = cx;
68  cy_ = cy;
69 }
70 
72 {
73  base_line_ = base_line;
74 }
75 
77 {
78  disparity_resolution_ = disparity_resolution;
79 }
80 
82 {
83  renderer.setClippingRange(near_clipping_plane_distance_, far_clipping_plane_distance_);
84  renderer.setBufferSize(width_, height_);
85  renderer.setCameraParameters(fx_, fy_, cx_, cy_);
86  // GLuint padding_coefficients_id = glGetUniformLocation (renderer.getProgramID (), "padding_coefficients");
87 
88  // // set device dependent padding coefficients
89  // glUniform3f (padding_coefficients_id, padding_coefficients_1_ * padding_scale_,
90  // padding_coefficients_2_ * padding_scale_,
91  // padding_coefficients_3_ * padding_scale_ + padding_offset_ );
92 }
93 
95 {
96  return padding_coefficients_;
97 }
98 
100 {
101  glUniform1f(glGetUniformLocation(renderer.getProgramID(), "near"), near_clipping_plane_distance_);
102  glUniform1f(glGetUniformLocation(renderer.getProgramID(), "far"), far_clipping_plane_distance_);
103 
104  renderer.setClippingRange(near_clipping_plane_distance_, far_clipping_plane_distance_);
105  renderer.setBufferSize(width_, height_);
106  renderer.setCameraParameters(fx_, fy_, cx_, cy_);
107 }
108 
109 // NOLINTNEXTLINE(readability-identifier-naming)
111  mesh_filter::StereoCameraModel::Parameters(640, 480, 0.4, 10.0, 525, 525, 319.5, 239.5, 0.075, 0.125);
112 
114  "#version 120\n"
115  "uniform vec3 padding_coefficients;"
116  "void main()"
117  "{"
118  " gl_FrontColor = gl_Color;"
119  " gl_BackColor = gl_Color;"
120  " vec4 vertex = gl_ModelViewMatrix * gl_Vertex;"
121  " vec3 normal = normalize(gl_NormalMatrix * gl_Normal);"
122  " float lambda = padding_coefficients.x * vertex.z * vertex.z + padding_coefficients.y * vertex.z + "
123  "padding_coefficients.z;"
124  " gl_Position = gl_ProjectionMatrix * (vertex + lambda * vec4(normal,0) );"
125  " gl_Position.y = -gl_Position.y;"
126  "}";
127 
128 const std::string mesh_filter::StereoCameraModel::RENDER_FRAGMENT_SHADER_SOURCE = "#version 120\n"
129  "void main()"
130  "{"
131  " gl_FragColor = gl_Color;"
132  "}";
133 
135  "#version 120\n"
136  "void main ()"
137  "{"
138  " gl_FrontColor = gl_Color;"
139  " gl_TexCoord[0] = gl_MultiTexCoord0;"
140  " gl_Position = gl_Vertex;"
141  " gl_Position.w = 1.0;"
142  "}";
143 
145  "#version 120\n"
146  "uniform sampler2D sensor;"
147  "uniform sampler2D depth;"
148  "uniform sampler2D label;"
149  "uniform float near;"
150  "uniform float far;"
151  "uniform float shadow_threshold;"
152  "const float shadowLabel = 1.0 / 255.0;"
153  "const float nearLabel = 2.0 / 255.0;"
154  "const float farLabel = 3.0 / 255.0;"
155  "float f_n = far - near;"
156  "float threshold = shadow_threshold / f_n;"
157  "void main()"
158  "{"
159  " float sValue = float(texture2D(sensor, gl_TexCoord[0].st));"
160  " if (sValue <= 0) {"
161  " gl_FragColor = vec4 (nearLabel, 0, 0, 0);"
162  " gl_FragDepth = 0;"
163  " }"
164  " else {"
165  " float dValue = float(texture2D(depth, gl_TexCoord[0].st));"
166  " float zValue = dValue * near / (far - dValue * f_n);"
167  " float diff = sValue - zValue;"
168  " if (diff < 0 && sValue < 1) {"
169  " gl_FragColor = vec4 (0, 0, 0, 0);"
170  " gl_FragDepth = float(texture2D(sensor, gl_TexCoord[0].st));"
171  " } else if (diff > threshold) {"
172  " gl_FragColor = vec4 (shadowLabel, 0, 0, 0);"
173  " gl_FragDepth = float(texture2D(sensor, gl_TexCoord[0].st));"
174  " } else if (sValue == 1) {"
175  " gl_FragColor = vec4 (farLabel, 0, 0, 0);"
176  " gl_FragDepth = float(texture2D(sensor, gl_TexCoord[0].st));"
177  " } else {"
178  " gl_FragColor = texture2D(label, gl_TexCoord[0].st);"
179  " gl_FragDepth = 0;"
180  " }"
181  " }"
182  "}";
Abstracts the OpenGL frame buffer objects, and provides an interface to render meshes,...
Definition: gl_renderer.h:63
void setBufferSize(unsigned width, unsigned height)
set the size of fram buffers
Definition: gl_renderer.cpp:84
void setCameraParameters(float fx, float fy, float cx, float cy)
set the camera parameters
void setClippingRange(float near, float far)
sets the near and far clipping plane distances in meters
Definition: gl_renderer.cpp:95
const GLuint & getProgramID() const
Abstract Interface defining Sensor Parameters.
Definition: sensor_model.h:61
Abstract Interface defining a sensor model for mesh filtering.
Definition: sensor_model.h:52
Parameters for Stereo-like devices.
void setBaseline(float base_line)
sets the base line = distance of the two projective devices (camera, projector-camera)
Parameters(unsigned width, unsigned height, float near_clipping_plane_distance, float far_clipping_plane_distance, float fx, float fy, float cx, float cy, float base_line, float disparity_resolution)
Constructor.
void setCameraParameters(float fx, float fy, float cx, float cy)
sets the camera parameters of the pinhole camera where the disparities were obtained....
void setDisparityResolution(float disparity_resolution)
the quantization of disparity values in pixels. Usually 1/16th or 1/8th for OpenNI compatible devices
void setRenderParameters(GLRenderer &renderer) const override
set the shader parameters required for the model rendering
void setFilterParameters(GLRenderer &renderer) const override
set the shader parameters required for the mesh filtering
SensorModel::Parameters * clone() const override
polymorphic clone method
const Eigen::Vector3f & getPaddingCoefficients() const override
returns the coefficients that are required for obtaining the padding for meshes
static const std::string RENDER_VERTEX_SHADER_SOURCE
source code of the vertex shader used to render the meshes
static const std::string RENDER_FRAGMENT_SHADER_SOURCE
source code of the fragment shader used to render the meshes
static const StereoCameraModel::Parameters & REGISTERED_PSDK_PARAMS
predefined sensor model for OpenNI compatible devices (e.g., PrimeSense, Kinect, Asus Xtion)
static const std::string FILTER_FRAGMENT_SHADER_SOURCE
source code of the fragment shader used to filter the depth map
static const std::string FILTER_VERTEX_SHADER_SOURCE
source code of the vertex shader used to filter the depth map