mesh_filter_test.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 #include <gtest/gtest.h>
00036 #include <moveit/mesh_filter/mesh_filter.h>
00037 #include <moveit/mesh_filter/stereo_camera_model.h>
00038 #include <geometric_shapes/shapes.h>
00039 #include <geometric_shapes/shape_operations.h>
00040 #include <eigen3/Eigen/Eigen>
00041 #include <vector>
00042 
00043 using namespace mesh_filter;
00044 using namespace Eigen;
00045 using namespace std;
00046 using namespace boost;
00047 
00048 namespace mesh_filter_test
00049 {
00050 template <typename Type>
00051 inline const Type getRandomNumber(const Type& min, const Type& max)
00052 {
00053   return Type(min + (max - min) * double(rand()) / double(RAND_MAX));
00054 }
00055 
00056 template <typename Type>
00057 class FilterTraits
00058 {
00059 public:
00060   static const GLushort FILTER_GL_TYPE = GL_ZERO;
00061 };
00062 
00063 template <>
00064 class FilterTraits<unsigned short>
00065 {
00066 public:
00067   static const GLushort FILTER_GL_TYPE = GL_UNSIGNED_SHORT;
00068   static const double ToMetricScale = 0.001;
00069 };
00070 
00071 template <>
00072 class FilterTraits<float>
00073 {
00074 public:
00075   static const GLushort FILTER_GL_TYPE = GL_FLOAT;
00076   static const double ToMetricScale = 1.0f;
00077 };
00078 
00079 template <typename Type>
00080 class MeshFilterTest : public testing::TestWithParam<double>
00081 {
00082   BOOST_STATIC_ASSERT_MSG(FilterTraits<Type>::FILTER_GL_TYPE != GL_ZERO, "Only \"float\" and \"unsigned short int\" "
00083                                                                          "are allowed.");
00084 
00085 public:
00086   MeshFilterTest(unsigned width = 500, unsigned height = 500, double near = 0.5, double far = 5.0, double shadow = 0.1,
00087                  double epsilon = 1e-7);
00088   void test();
00089   void setMeshDistance(double distance)
00090   {
00091     distance_ = distance;
00092   }
00093 
00094 private:
00095   shapes::Mesh createMesh(double z) const;
00096   bool transform_callback(MeshHandle handle, Affine3d& transform) const;
00097   void getGroundTruth(unsigned int* labels, float* depth) const;
00098   const unsigned int width_;
00099   const unsigned int height_;
00100   const double near_;
00101   const double far_;
00102   const double shadow_;
00103   const double epsilon_;
00104   StereoCameraModel::Parameters sensor_parameters_;
00105   MeshFilter<StereoCameraModel> filter_;
00106   MeshHandle handle_;
00107   vector<Type> sensor_data_;
00108   double distance_;
00109 };
00110 
00111 template <typename Type>
00112 MeshFilterTest<Type>::MeshFilterTest(unsigned width, unsigned height, double near, double far, double shadow,
00113                                      double epsilon)
00114   : width_(width)
00115   , height_(height)
00116   , near_(near)
00117   , far_(far)
00118   , shadow_(shadow)
00119   , epsilon_(epsilon)
00120   , sensor_parameters_(width, height, near_, far_, width >> 1, height >> 1, width >> 1, height >> 1, 0.1, 0.1)
00121   , filter_(boost::bind(&MeshFilterTest<Type>::transform_callback, this, _1, _2), sensor_parameters_)
00122   , sensor_data_(width_ * height_)
00123   , distance_(0.0)
00124 {
00125   filter_.setShadowThreshold(shadow_);
00126   // no padding
00127   filter_.setPaddingOffset(0.0);
00128   filter_.setPaddingScale(0.0);
00129 
00130   // create a large plane that covers the whole visible area -> no boundaries
00131 
00132   shapes::Mesh mesh = createMesh(0);
00133   handle_ = filter_.addMesh(mesh);
00134 
00135   // make it random but reproducable
00136   srand(0);
00137   Type t_near = near_ / FilterTraits<Type>::ToMetricScale;
00138   Type t_far = far_ / FilterTraits<Type>::ToMetricScale;
00139   for (typename vector<Type>::iterator sIt = sensor_data_.begin(); sIt != sensor_data_.end(); ++sIt)
00140   {
00141     do
00142     {
00143       *sIt = getRandomNumber<Type>(0.0, 10.0 / FilterTraits<Type>::ToMetricScale);
00144     } while (*sIt == t_near || *sIt == t_far);
00145   }
00146 }
00147 
00148 template <typename Type>
00149 shapes::Mesh MeshFilterTest<Type>::createMesh(double z) const
00150 {
00151   shapes::Mesh mesh(4, 4);
00152   mesh.vertices[0] = -5;
00153   mesh.vertices[1] = -5;
00154   mesh.vertices[2] = z;
00155 
00156   mesh.vertices[3] = -5;
00157   mesh.vertices[4] = 5;
00158   mesh.vertices[5] = z;
00159 
00160   mesh.vertices[6] = 5;
00161   mesh.vertices[7] = 5;
00162   mesh.vertices[8] = z;
00163 
00164   mesh.vertices[9] = 5;
00165   mesh.vertices[10] = -5;
00166   mesh.vertices[11] = z;
00167 
00168   mesh.triangles[0] = 0;
00169   mesh.triangles[1] = 3;
00170   mesh.triangles[2] = 2;
00171 
00172   mesh.triangles[3] = 0;
00173   mesh.triangles[4] = 2;
00174   mesh.triangles[5] = 1;
00175 
00176   mesh.triangles[6] = 0;
00177   mesh.triangles[7] = 2;
00178   mesh.triangles[8] = 3;
00179 
00180   mesh.triangles[9] = 0;
00181   mesh.triangles[10] = 1;
00182   mesh.triangles[11] = 2;
00183 
00184   mesh.vertex_normals[0] = 0;
00185   mesh.vertex_normals[1] = 0;
00186   mesh.vertex_normals[2] = 1;
00187 
00188   mesh.vertex_normals[3] = 0;
00189   mesh.vertex_normals[4] = 0;
00190   mesh.vertex_normals[5] = 1;
00191 
00192   mesh.vertex_normals[6] = 0;
00193   mesh.vertex_normals[7] = 0;
00194   mesh.vertex_normals[8] = 1;
00195 
00196   mesh.vertex_normals[9] = 0;
00197   mesh.vertex_normals[10] = 0;
00198   mesh.vertex_normals[11] = 1;
00199 
00200   return mesh;
00201 }
00202 
00203 template <typename Type>
00204 bool MeshFilterTest<Type>::transform_callback(MeshHandle handle, Affine3d& transform) const
00205 {
00206   transform = Affine3d::Identity();
00207   if (handle == handle_)
00208     transform.translation() = Vector3d(0, 0, distance_);
00209   return true;
00210 }
00211 
00212 template <typename Type>
00213 void MeshFilterTest<Type>::test()
00214 {
00215   shapes::Mesh mesh = createMesh(0);
00216   mesh_filter::MeshHandle handle = filter_.addMesh(mesh);
00217   filter_.filter(&sensor_data_[0], FilterTraits<Type>::FILTER_GL_TYPE, false);
00218 
00219   vector<float> gt_depth(width_ * height_);
00220   vector<unsigned int> gt_labels(width_ * height_);
00221   getGroundTruth(&gt_labels[0], &gt_depth[0]);
00222 
00223   vector<float> filtered_depth(width_ * height_);
00224   vector<unsigned int> filtered_labels(width_ * height_);
00225   filter_.getFilteredDepth(&filtered_depth[0]);
00226   filter_.getFilteredLabels(&filtered_labels[0]);
00227 
00228   for (unsigned idx = 0; idx < width_ * height_; ++idx)
00229   {
00230     // Only test if we are not very close to boundaries of object meshes and shadow-boundaries.
00231     float sensor_depth = sensor_data_[idx] * FilterTraits<Type>::ToMetricScale;
00232     if (fabs(sensor_depth - distance_ - shadow_) > epsilon_ && fabs(sensor_depth - distance_) > epsilon_)
00233     {
00234       ASSERT_FLOAT_EQ(filtered_depth[idx], gt_depth[idx]);
00235       ASSERT_EQ(filtered_labels[idx], gt_labels[idx]);
00236     }
00237   }
00238   filter_.removeMesh(handle);
00239 }
00240 
00241 template <typename Type>
00242 void MeshFilterTest<Type>::getGroundTruth(unsigned int* labels, float* depth) const
00243 {
00244   const double scale = FilterTraits<Type>::ToMetricScale;
00245   if (distance_ <= near_ || distance_ >= far_)
00246   {
00247     // no filtering is done -> no shadow values or label values
00248     for (unsigned yIdx = 0, idx = 0; yIdx < height_; ++yIdx)
00249     {
00250       for (unsigned xIdx = 0; xIdx < width_; ++xIdx, ++idx)
00251       {
00252         depth[idx] = double(sensor_data_[idx]) * scale;
00253         if (depth[idx] < near_)
00254           labels[idx] = MeshFilterBase::NearClip;
00255         else if (depth[idx] >= far_)
00256           labels[idx] = MeshFilterBase::FarClip;
00257         else
00258           labels[idx] = MeshFilterBase::Background;
00259 
00260         if (depth[idx] <= near_ || depth[idx] >= far_)
00261           depth[idx] = 0;
00262       }
00263     }
00264   }
00265   else
00266   {
00267     for (unsigned yIdx = 0, idx = 0; yIdx < height_; ++yIdx)
00268     {
00269       for (unsigned xIdx = 0; xIdx < width_; ++xIdx, ++idx)
00270       {
00271         depth[idx] = double(sensor_data_[idx]) * scale;
00272 
00273         if (depth[idx] < near_)
00274         {
00275           labels[idx] = MeshFilterBase::NearClip;
00276           depth[idx] = 0;
00277         }
00278         else
00279         {
00280           double diff = depth[idx] - distance_;
00281           if (diff < 0 && depth[idx] < far_)
00282             labels[idx] = MeshFilterBase::Background;
00283           else if (diff > shadow_)
00284             labels[idx] = MeshFilterBase::Shadow;
00285           else if (depth[idx] >= far_)
00286             labels[idx] = MeshFilterBase::FarClip;
00287           else
00288           {
00289             labels[idx] = MeshFilterBase::FirstLabel;
00290             depth[idx] = 0;
00291           }
00292 
00293           if (depth[idx] >= far_)
00294             depth[idx] = 0;
00295         }
00296       }
00297     }
00298   }
00299 }
00300 
00301 }  // namespace mesh_filter_test
00302 
00303 typedef mesh_filter_test::MeshFilterTest<float> MeshFilterTestFloat;
00304 TEST_P(MeshFilterTestFloat, float)
00305 {
00306   this->setMeshDistance(this->GetParam());
00307   this->test();
00308 }
00309 INSTANTIATE_TEST_CASE_P(float_test, MeshFilterTestFloat, ::testing::Range<double>(0.0f, 6.0f, 0.5f));
00310 
00311 typedef mesh_filter_test::MeshFilterTest<unsigned short> MeshFilterTestUnsignedShort;
00312 TEST_P(MeshFilterTestUnsignedShort, unsigned_short)
00313 {
00314   this->setMeshDistance(this->GetParam());
00315   this->test();
00316 }
00317 INSTANTIATE_TEST_CASE_P(ushort_test, MeshFilterTestUnsignedShort, ::testing::Range<double>(0.0f, 6.0f, 0.5f));
00318 
00319 int main(int argc, char** argv)
00320 {
00321   testing::InitGoogleTest(&argc, argv);
00322   int arg;
00323 
00324   return RUN_ALL_TESTS();
00325 }


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