stereo_camera_model.cpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Suat Gedikli */

#include <moveit/mesh_filter/stereo_camera_model.h>
#include <moveit/mesh_filter/gl_renderer.h>

using namespace std;

mesh_filter::StereoCameraModel::Parameters::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)
: SensorModel::Parameters (width, height, near_clipping_plane_distance, far_clipping_plane_distance)
, fx_ (fx)
, fy_ (fy)
, cx_ (cx)
, cy_ (cy)
, base_line_ (base_line)
, disparity_resolution_ (disparity_resolution)
, padding_coefficients_ (Eigen::Vector3f (disparity_resolution_ / (fx_ * base_line_), 0, 0))
{
}

mesh_filter::StereoCameraModel::Parameters::~Parameters ()
{
}

mesh_filter::SensorModel::Parameters* mesh_filter::StereoCameraModel::Parameters::clone () const
{
  return new StereoCameraModel::Parameters (width_, height_,
                                            near_clipping_plane_distance_, far_clipping_plane_distance_,
                                            fx_, fy_, cx_, cy_, base_line_, disparity_resolution_);
}

void mesh_filter::StereoCameraModel::Parameters::setCameraParameters (float fx, float fy, float cx, float cy)
{
  fx_ = fx;
  fy_ = fy;
  cx_ = cx;
  cy_ = cy;
}

void mesh_filter::StereoCameraModel::Parameters::setBaseline (float base_line)
{
  base_line_ = base_line;
}

void mesh_filter::StereoCameraModel::Parameters::setDisparityResolution (float disparity_resolution)
{
  disparity_resolution_ = disparity_resolution;
}

void mesh_filter::StereoCameraModel::Parameters::setRenderParameters (GLRenderer& renderer) const
{
  renderer.setClippingRange (near_clipping_plane_distance_, far_clipping_plane_distance_);
  renderer.setBufferSize (width_, height_);
  renderer.setCameraParameters (fx_, fy_, cx_, cy_);
  //GLuint padding_coefficients_id = glGetUniformLocation (renderer.getProgramID (), "padding_coefficients");

//  // set device dependent padding coefficients
//  glUniform3f (padding_coefficients_id, padding_coefficients_1_ * padding_scale_,
//                                        padding_coefficients_2_ * padding_scale_,
//                                        padding_coefficients_3_ * padding_scale_  + padding_offset_ );
}

const Eigen::Vector3f& mesh_filter::StereoCameraModel::Parameters::getPaddingCoefficients () const
{
  return padding_coefficients_;
}

void mesh_filter::StereoCameraModel::Parameters::setFilterParameters (GLRenderer& renderer) const
{
  glUniform1f (glGetUniformLocation (renderer.getProgramID (), "near"), near_clipping_plane_distance_);
  glUniform1f (glGetUniformLocation (renderer.getProgramID (), "far"), far_clipping_plane_distance_);

  renderer.setClippingRange (near_clipping_plane_distance_, far_clipping_plane_distance_);
  renderer.setBufferSize (width_, height_);
  renderer.setCameraParameters (fx_, fy_, cx_, cy_);
}

const mesh_filter::StereoCameraModel::Parameters& mesh_filter::StereoCameraModel::RegisteredPSDKParams =
      mesh_filter::StereoCameraModel::Parameters (640, 480, 0.4, 10.0, 525, 525, 319.5, 239.5, 0.075, 0.125);

const string mesh_filter::StereoCameraModel::renderVertexShaderSource =
        "#version 120\n"
        "uniform vec3 padding_coefficients;"
        "void main()"
        "{"
        "  gl_FrontColor = gl_Color;"
        "  gl_BackColor = gl_Color;"
        "  vec4 vertex = gl_ModelViewMatrix * gl_Vertex;"
        "  vec3 normal = normalize(gl_NormalMatrix * gl_Normal);"
        "  float lambda = padding_coefficients.x * vertex.z * vertex.z + padding_coefficients.y * vertex.z + padding_coefficients.z;"
        "  gl_Position = gl_ProjectionMatrix * (vertex + lambda * vec4(normal,0) );"
        "  gl_Position.y = -gl_Position.y;"
        "}";

const string mesh_filter::StereoCameraModel::renderFragmentShaderSource =
        "#version 120\n"
        "void main()"
        "{"
        "  gl_FragColor = gl_Color;"
        "}";

const string mesh_filter::StereoCameraModel::filterVertexShaderSource =
        "#version 120\n"
        "void main ()"
        "{"
        "     gl_FrontColor = gl_Color;"
        "     gl_TexCoord[0] = gl_MultiTexCoord0;"
        "     gl_Position = gl_Vertex;"
        "  gl_Position.w = 1.0;"
        "}";

const string mesh_filter::StereoCameraModel::filterFragmentShaderSource =
        "#version 120\n"
        "uniform sampler2D sensor;"
        "uniform sampler2D depth;"
        "uniform sampler2D label;"
        "uniform float near;"
        "uniform float far;"
        "uniform float shadow_threshold;"
        "const float shadowLabel = 1.0 / 255.0;"
        "const float nearLabel = 2.0 / 255.0;"
        "const float farLabel = 3.0 / 255.0;"
        "float f_n = far - near;"
        "float threshold = shadow_threshold / f_n;"
        "void main()"
        "{"
        " float sValue = float(texture2D(sensor, gl_TexCoord[0].st));"
        " if (sValue <= 0) {"
        "   gl_FragColor = vec4 (nearLabel, 0, 0, 0);"
        "   gl_FragDepth = 0;"
        " }"
        " else {"
        "      float dValue = float(texture2D(depth, gl_TexCoord[0].st));"
        "      float zValue = dValue * near / (far - dValue * f_n);"
        "      float diff = sValue - zValue;"
        "      if (diff < 0 && sValue < 1) {"
        "          gl_FragColor = vec4 (0, 0, 0, 0);"
        "          gl_FragDepth = float(texture2D(sensor, gl_TexCoord[0].st));"
        "      }    else if (diff > threshold) {"
        "          gl_FragColor = vec4 (shadowLabel, 0, 0, 0);"
        "          gl_FragDepth = float(texture2D(sensor, gl_TexCoord[0].st));"
        "      }    else if (sValue == 1) {"
        "          gl_FragColor = vec4 (farLabel, 0, 0, 0);"
        "          gl_FragDepth = float(texture2D(sensor, gl_TexCoord[0].st));"
        "   } else {"
        "          gl_FragColor = texture2D(label, gl_TexCoord[0].st);"
        "          gl_FragDepth = 0;"
        "      }"
        " }"
        "}";