00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/mesh_filter/stereo_camera_model.h>
00038 #include <moveit/mesh_filter/gl_renderer.h>
00039
00040 using namespace std;
00041
00042 mesh_filter::StereoCameraModel::Parameters::Parameters(unsigned width, unsigned height,
00043 float near_clipping_plane_distance,
00044 float far_clipping_plane_distance, float fx, float fy, float cx,
00045 float cy, float base_line, float disparity_resolution)
00046 : SensorModel::Parameters(width, height, near_clipping_plane_distance, far_clipping_plane_distance)
00047 , fx_(fx)
00048 , fy_(fy)
00049 , cx_(cx)
00050 , cy_(cy)
00051 , base_line_(base_line)
00052 , disparity_resolution_(disparity_resolution)
00053 , padding_coefficients_(Eigen::Vector3f(disparity_resolution_ / (fx_ * base_line_), 0, 0))
00054 {
00055 }
00056
00057 mesh_filter::StereoCameraModel::Parameters::~Parameters()
00058 {
00059 }
00060
00061 mesh_filter::SensorModel::Parameters* mesh_filter::StereoCameraModel::Parameters::clone() const
00062 {
00063 return new StereoCameraModel::Parameters(width_, height_, near_clipping_plane_distance_, far_clipping_plane_distance_,
00064 fx_, fy_, cx_, cy_, base_line_, disparity_resolution_);
00065 }
00066
00067 void mesh_filter::StereoCameraModel::Parameters::setCameraParameters(float fx, float fy, float cx, float cy)
00068 {
00069 fx_ = fx;
00070 fy_ = fy;
00071 cx_ = cx;
00072 cy_ = cy;
00073 }
00074
00075 void mesh_filter::StereoCameraModel::Parameters::setBaseline(float base_line)
00076 {
00077 base_line_ = base_line;
00078 }
00079
00080 void mesh_filter::StereoCameraModel::Parameters::setDisparityResolution(float disparity_resolution)
00081 {
00082 disparity_resolution_ = disparity_resolution;
00083 }
00084
00085 void mesh_filter::StereoCameraModel::Parameters::setRenderParameters(GLRenderer& renderer) const
00086 {
00087 renderer.setClippingRange(near_clipping_plane_distance_, far_clipping_plane_distance_);
00088 renderer.setBufferSize(width_, height_);
00089 renderer.setCameraParameters(fx_, fy_, cx_, cy_);
00090
00091
00092
00093
00094
00095
00096 }
00097
00098 const Eigen::Vector3f& mesh_filter::StereoCameraModel::Parameters::getPaddingCoefficients() const
00099 {
00100 return padding_coefficients_;
00101 }
00102
00103 void mesh_filter::StereoCameraModel::Parameters::setFilterParameters(GLRenderer& renderer) const
00104 {
00105 glUniform1f(glGetUniformLocation(renderer.getProgramID(), "near"), near_clipping_plane_distance_);
00106 glUniform1f(glGetUniformLocation(renderer.getProgramID(), "far"), far_clipping_plane_distance_);
00107
00108 renderer.setClippingRange(near_clipping_plane_distance_, far_clipping_plane_distance_);
00109 renderer.setBufferSize(width_, height_);
00110 renderer.setCameraParameters(fx_, fy_, cx_, cy_);
00111 }
00112
00113 const mesh_filter::StereoCameraModel::Parameters& mesh_filter::StereoCameraModel::RegisteredPSDKParams =
00114 mesh_filter::StereoCameraModel::Parameters(640, 480, 0.4, 10.0, 525, 525, 319.5, 239.5, 0.075, 0.125);
00115
00116 const string mesh_filter::StereoCameraModel::renderVertexShaderSource =
00117 "#version 120\n"
00118 "uniform vec3 padding_coefficients;"
00119 "void main()"
00120 "{"
00121 " gl_FrontColor = gl_Color;"
00122 " gl_BackColor = gl_Color;"
00123 " vec4 vertex = gl_ModelViewMatrix * gl_Vertex;"
00124 " vec3 normal = normalize(gl_NormalMatrix * gl_Normal);"
00125 " float lambda = padding_coefficients.x * vertex.z * vertex.z + padding_coefficients.y * vertex.z + "
00126 "padding_coefficients.z;"
00127 " gl_Position = gl_ProjectionMatrix * (vertex + lambda * vec4(normal,0) );"
00128 " gl_Position.y = -gl_Position.y;"
00129 "}";
00130
00131 const string mesh_filter::StereoCameraModel::renderFragmentShaderSource = "#version 120\n"
00132 "void main()"
00133 "{"
00134 " gl_FragColor = gl_Color;"
00135 "}";
00136
00137 const string mesh_filter::StereoCameraModel::filterVertexShaderSource = "#version 120\n"
00138 "void main ()"
00139 "{"
00140 " gl_FrontColor = gl_Color;"
00141 " gl_TexCoord[0] = gl_MultiTexCoord0;"
00142 " gl_Position = gl_Vertex;"
00143 " gl_Position.w = 1.0;"
00144 "}";
00145
00146 const string mesh_filter::StereoCameraModel::filterFragmentShaderSource =
00147 "#version 120\n"
00148 "uniform sampler2D sensor;"
00149 "uniform sampler2D depth;"
00150 "uniform sampler2D label;"
00151 "uniform float near;"
00152 "uniform float far;"
00153 "uniform float shadow_threshold;"
00154 "const float shadowLabel = 1.0 / 255.0;"
00155 "const float nearLabel = 2.0 / 255.0;"
00156 "const float farLabel = 3.0 / 255.0;"
00157 "float f_n = far - near;"
00158 "float threshold = shadow_threshold / f_n;"
00159 "void main()"
00160 "{"
00161 " float sValue = float(texture2D(sensor, gl_TexCoord[0].st));"
00162 " if (sValue <= 0) {"
00163 " gl_FragColor = vec4 (nearLabel, 0, 0, 0);"
00164 " gl_FragDepth = 0;"
00165 " }"
00166 " else {"
00167 " float dValue = float(texture2D(depth, gl_TexCoord[0].st));"
00168 " float zValue = dValue * near / (far - dValue * f_n);"
00169 " float diff = sValue - zValue;"
00170 " if (diff < 0 && sValue < 1) {"
00171 " gl_FragColor = vec4 (0, 0, 0, 0);"
00172 " gl_FragDepth = float(texture2D(sensor, gl_TexCoord[0].st));"
00173 " } else if (diff > threshold) {"
00174 " gl_FragColor = vec4 (shadowLabel, 0, 0, 0);"
00175 " gl_FragDepth = float(texture2D(sensor, gl_TexCoord[0].st));"
00176 " } else if (sValue == 1) {"
00177 " gl_FragColor = vec4 (farLabel, 0, 0, 0);"
00178 " gl_FragDepth = float(texture2D(sensor, gl_TexCoord[0].st));"
00179 " } else {"
00180 " gl_FragColor = texture2D(label, gl_TexCoord[0].st);"
00181 " gl_FragDepth = 0;"
00182 " }"
00183 " }"
00184 "}";