mesh_filter_test.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.
 *********************************************************************/

#include <gtest/gtest.h>
#include <moveit/mesh_filter/mesh_filter.h>
#include <moveit/mesh_filter/stereo_camera_model.h>
#include <geometric_shapes/shapes.h>
#include <geometric_shapes/shape_operations.h>
#include <eigen3/Eigen/Eigen>
#include <vector>

using namespace mesh_filter;
using namespace Eigen;
using namespace std;
using namespace boost;

namespace mesh_filter_test
{
template <typename Type>
inline const Type getRandomNumber(const Type& min, const Type& max)
{
  return Type(min + (max - min) * double(rand()) / double(RAND_MAX));
}

template <typename Type>
class FilterTraits
{
public:
  static const GLushort FILTER_GL_TYPE = GL_ZERO;
};

template <>
class FilterTraits<unsigned short>
{
public:
  static const GLushort FILTER_GL_TYPE = GL_UNSIGNED_SHORT;
  static const double ToMetricScale = 0.001;
};

template <>
class FilterTraits<float>
{
public:
  static const GLushort FILTER_GL_TYPE = GL_FLOAT;
  static const double ToMetricScale = 1.0f;
};

template <typename Type>
class MeshFilterTest : public testing::TestWithParam<double>
{
  BOOST_STATIC_ASSERT_MSG(FilterTraits<Type>::FILTER_GL_TYPE != GL_ZERO, "Only \"float\" and \"unsigned short int\" "
                                                                         "are allowed.");

public:
  MeshFilterTest(unsigned width = 500, unsigned height = 500, double near = 0.5, double far = 5.0, double shadow = 0.1,
                 double epsilon = 1e-7);
  void test();
  void setMeshDistance(double distance)
  {
    distance_ = distance;
  }

private:
  shapes::Mesh createMesh(double z) const;
  bool transform_callback(MeshHandle handle, Affine3d& transform) const;
  void getGroundTruth(unsigned int* labels, float* depth) const;
  const unsigned int width_;
  const unsigned int height_;
  const double near_;
  const double far_;
  const double shadow_;
  const double epsilon_;
  StereoCameraModel::Parameters sensor_parameters_;
  MeshFilter<StereoCameraModel> filter_;
  MeshHandle handle_;
  vector<Type> sensor_data_;
  double distance_;
};

template <typename Type>
MeshFilterTest<Type>::MeshFilterTest(unsigned width, unsigned height, double near, double far, double shadow,
                                     double epsilon)
  : width_(width)
  , height_(height)
  , near_(near)
  , far_(far)
  , shadow_(shadow)
  , epsilon_(epsilon)
  , sensor_parameters_(width, height, near_, far_, width >> 1, height >> 1, width >> 1, height >> 1, 0.1, 0.1)
  , filter_(boost::bind(&MeshFilterTest<Type>::transform_callback, this, _1, _2), sensor_parameters_)
  , sensor_data_(width_ * height_)
  , distance_(0.0)
{
  filter_.setShadowThreshold(shadow_);
  // no padding
  filter_.setPaddingOffset(0.0);
  filter_.setPaddingScale(0.0);

  // create a large plane that covers the whole visible area -> no boundaries

  shapes::Mesh mesh = createMesh(0);
  handle_ = filter_.addMesh(mesh);

  // make it random but reproducable
  srand(0);
  Type t_near = near_ / FilterTraits<Type>::ToMetricScale;
  Type t_far = far_ / FilterTraits<Type>::ToMetricScale;
  for (typename vector<Type>::iterator sIt = sensor_data_.begin(); sIt != sensor_data_.end(); ++sIt)
  {
    do
    {
      *sIt = getRandomNumber<Type>(0.0, 10.0 / FilterTraits<Type>::ToMetricScale);
    } while (*sIt == t_near || *sIt == t_far);
  }
}

template <typename Type>
shapes::Mesh MeshFilterTest<Type>::createMesh(double z) const
{
  shapes::Mesh mesh(4, 4);
  mesh.vertices[0] = -5;
  mesh.vertices[1] = -5;
  mesh.vertices[2] = z;

  mesh.vertices[3] = -5;
  mesh.vertices[4] = 5;
  mesh.vertices[5] = z;

  mesh.vertices[6] = 5;
  mesh.vertices[7] = 5;
  mesh.vertices[8] = z;

  mesh.vertices[9] = 5;
  mesh.vertices[10] = -5;
  mesh.vertices[11] = z;

  mesh.triangles[0] = 0;
  mesh.triangles[1] = 3;
  mesh.triangles[2] = 2;

  mesh.triangles[3] = 0;
  mesh.triangles[4] = 2;
  mesh.triangles[5] = 1;

  mesh.triangles[6] = 0;
  mesh.triangles[7] = 2;
  mesh.triangles[8] = 3;

  mesh.triangles[9] = 0;
  mesh.triangles[10] = 1;
  mesh.triangles[11] = 2;

  mesh.vertex_normals[0] = 0;
  mesh.vertex_normals[1] = 0;
  mesh.vertex_normals[2] = 1;

  mesh.vertex_normals[3] = 0;
  mesh.vertex_normals[4] = 0;
  mesh.vertex_normals[5] = 1;

  mesh.vertex_normals[6] = 0;
  mesh.vertex_normals[7] = 0;
  mesh.vertex_normals[8] = 1;

  mesh.vertex_normals[9] = 0;
  mesh.vertex_normals[10] = 0;
  mesh.vertex_normals[11] = 1;

  return mesh;
}

template <typename Type>
bool MeshFilterTest<Type>::transform_callback(MeshHandle handle, Affine3d& transform) const
{
  transform = Affine3d::Identity();
  if (handle == handle_)
    transform.translation() = Vector3d(0, 0, distance_);
  return true;
}

template <typename Type>
void MeshFilterTest<Type>::test()
{
  shapes::Mesh mesh = createMesh(0);
  mesh_filter::MeshHandle handle = filter_.addMesh(mesh);
  filter_.filter(&sensor_data_[0], FilterTraits<Type>::FILTER_GL_TYPE, false);

  vector<float> gt_depth(width_ * height_);
  vector<unsigned int> gt_labels(width_ * height_);
  getGroundTruth(&gt_labels[0], &gt_depth[0]);

  vector<float> filtered_depth(width_ * height_);
  vector<unsigned int> filtered_labels(width_ * height_);
  filter_.getFilteredDepth(&filtered_depth[0]);
  filter_.getFilteredLabels(&filtered_labels[0]);

  for (unsigned idx = 0; idx < width_ * height_; ++idx)
  {
    // Only test if we are not very close to boundaries of object meshes and shadow-boundaries.
    float sensor_depth = sensor_data_[idx] * FilterTraits<Type>::ToMetricScale;
    if (fabs(sensor_depth - distance_ - shadow_) > epsilon_ && fabs(sensor_depth - distance_) > epsilon_)
    {
      ASSERT_FLOAT_EQ(filtered_depth[idx], gt_depth[idx]);
      ASSERT_EQ(filtered_labels[idx], gt_labels[idx]);
    }
  }
  filter_.removeMesh(handle);
}

template <typename Type>
void MeshFilterTest<Type>::getGroundTruth(unsigned int* labels, float* depth) const
{
  const double scale = FilterTraits<Type>::ToMetricScale;
  if (distance_ <= near_ || distance_ >= far_)
  {
    // no filtering is done -> no shadow values or label values
    for (unsigned yIdx = 0, idx = 0; yIdx < height_; ++yIdx)
    {
      for (unsigned xIdx = 0; xIdx < width_; ++xIdx, ++idx)
      {
        depth[idx] = double(sensor_data_[idx]) * scale;
        if (depth[idx] < near_)
          labels[idx] = MeshFilterBase::NearClip;
        else if (depth[idx] >= far_)
          labels[idx] = MeshFilterBase::FarClip;
        else
          labels[idx] = MeshFilterBase::Background;

        if (depth[idx] <= near_ || depth[idx] >= far_)
          depth[idx] = 0;
      }
    }
  }
  else
  {
    for (unsigned yIdx = 0, idx = 0; yIdx < height_; ++yIdx)
    {
      for (unsigned xIdx = 0; xIdx < width_; ++xIdx, ++idx)
      {
        depth[idx] = double(sensor_data_[idx]) * scale;

        if (depth[idx] < near_)
        {
          labels[idx] = MeshFilterBase::NearClip;
          depth[idx] = 0;
        }
        else
        {
          double diff = depth[idx] - distance_;
          if (diff < 0 && depth[idx] < far_)
            labels[idx] = MeshFilterBase::Background;
          else if (diff > shadow_)
            labels[idx] = MeshFilterBase::Shadow;
          else if (depth[idx] >= far_)
            labels[idx] = MeshFilterBase::FarClip;
          else
          {
            labels[idx] = MeshFilterBase::FirstLabel;
            depth[idx] = 0;
          }

          if (depth[idx] >= far_)
            depth[idx] = 0;
        }
      }
    }
  }
}

}  // namespace mesh_filter_test

typedef mesh_filter_test::MeshFilterTest<float> MeshFilterTestFloat;
TEST_P(MeshFilterTestFloat, float)
{
  this->setMeshDistance(this->GetParam());
  this->test();
}
INSTANTIATE_TEST_CASE_P(float_test, MeshFilterTestFloat, ::testing::Range<double>(0.0f, 6.0f, 0.5f));

typedef mesh_filter_test::MeshFilterTest<unsigned short> MeshFilterTestUnsignedShort;
TEST_P(MeshFilterTestUnsignedShort, unsigned_short)
{
  this->setMeshDistance(this->GetParam());
  this->test();
}
INSTANTIATE_TEST_CASE_P(ushort_test, MeshFilterTestUnsignedShort, ::testing::Range<double>(0.0f, 6.0f, 0.5f));

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  int arg;

  return RUN_ALL_TESTS();
}