stereo_camera_model.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Suat Gedikli */
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   // GLuint padding_coefficients_id = glGetUniformLocation (renderer.getProgramID (), "padding_coefficients");
00091 
00092   //  // set device dependent padding coefficients
00093   //  glUniform3f (padding_coefficients_id, padding_coefficients_1_ * padding_scale_,
00094   //                                        padding_coefficients_2_ * padding_scale_,
00095   //                                        padding_coefficients_3_ * padding_scale_  + padding_offset_ );
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     "}";


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Wed Jun 19 2019 19:24:12