mesh_filter_test.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #include <gtest/gtest.h>
40 #include <eigen3/Eigen/Eigen>
41 #include <vector>
42 
43 using namespace mesh_filter;
44 using namespace Eigen;
45 using namespace std;
46 using namespace boost;
47 
49 {
50 template <typename Type>
51 inline const Type getRandomNumber(const Type& min, const Type& max)
52 {
53  return Type(min + (max - min) * double(rand()) / double(RAND_MAX));
54 }
55 
56 template <typename Type>
58 {
59 public:
60  static const GLushort FILTER_GL_TYPE = GL_ZERO;
61 };
62 
63 template <>
64 class FilterTraits<unsigned short>
65 {
66 public:
67  static const GLushort FILTER_GL_TYPE = GL_UNSIGNED_SHORT;
68  static constexpr double ToMetricScale = 0.001;
69 };
70 
71 template <>
72 class FilterTraits<float>
73 {
74 public:
75  static const GLushort FILTER_GL_TYPE = GL_FLOAT;
76  static constexpr double ToMetricScale = 1.0f;
77 };
78 
79 template <typename Type>
80 class MeshFilterTest : public testing::TestWithParam<double>
81 {
82  BOOST_STATIC_ASSERT_MSG(FilterTraits<Type>::FILTER_GL_TYPE != GL_ZERO, "Only \"float\" and \"unsigned short int\" "
83  "are allowed.");
84 
85 public:
86  MeshFilterTest(unsigned width = 500, unsigned height = 500, double near = 0.5, double far = 5.0, double shadow = 0.1,
87  double epsilon = 1e-7);
88  void test();
89  void setMeshDistance(double distance)
90  {
91  distance_ = distance;
92  }
93 
94 private:
95  shapes::Mesh createMesh(double z) const;
96  bool transform_callback(MeshHandle handle, Affine3d& transform) const;
97  void getGroundTruth(unsigned int* labels, float* depth) const;
98  const unsigned int width_;
99  const unsigned int height_;
100  const double near_;
101  const double far_;
102  const double shadow_;
103  const double epsilon_;
107  vector<Type> sensor_data_;
108  double distance_;
109 };
110 
111 template <typename Type>
112 MeshFilterTest<Type>::MeshFilterTest(unsigned width, unsigned height, double near, double far, double shadow,
113  double epsilon)
114  : width_(width)
115  , height_(height)
116  , near_(near)
117  , far_(far)
118  , shadow_(shadow)
119  , epsilon_(epsilon)
120  , sensor_parameters_(width, height, near_, far_, width >> 1, height >> 1, width >> 1, height >> 1, 0.1, 0.1)
121  , filter_(boost::bind(&MeshFilterTest<Type>::transform_callback, this, _1, _2), sensor_parameters_)
122  , sensor_data_(width_ * height_)
123  , distance_(0.0)
124 {
126  // no padding
129 
130  // create a large plane that covers the whole visible area -> no boundaries
131 
132  shapes::Mesh mesh = createMesh(0);
133  handle_ = filter_.addMesh(mesh);
134 
135  // make it random but reproducable
136  srand(0);
139  for (typename vector<Type>::iterator sIt = sensor_data_.begin(); sIt != sensor_data_.end(); ++sIt)
140  {
141  do
142  {
143  *sIt = getRandomNumber<Type>(0.0, 10.0 / FilterTraits<Type>::ToMetricScale);
144  } while (*sIt == t_near || *sIt == t_far);
145  }
146 }
147 
148 template <typename Type>
150 {
151  shapes::Mesh mesh(4, 4);
152  mesh.vertices[0] = -5;
153  mesh.vertices[1] = -5;
154  mesh.vertices[2] = z;
155 
156  mesh.vertices[3] = -5;
157  mesh.vertices[4] = 5;
158  mesh.vertices[5] = z;
159 
160  mesh.vertices[6] = 5;
161  mesh.vertices[7] = 5;
162  mesh.vertices[8] = z;
163 
164  mesh.vertices[9] = 5;
165  mesh.vertices[10] = -5;
166  mesh.vertices[11] = z;
167 
168  mesh.triangles[0] = 0;
169  mesh.triangles[1] = 3;
170  mesh.triangles[2] = 2;
171 
172  mesh.triangles[3] = 0;
173  mesh.triangles[4] = 2;
174  mesh.triangles[5] = 1;
175 
176  mesh.triangles[6] = 0;
177  mesh.triangles[7] = 2;
178  mesh.triangles[8] = 3;
179 
180  mesh.triangles[9] = 0;
181  mesh.triangles[10] = 1;
182  mesh.triangles[11] = 2;
183 
184  mesh.vertex_normals[0] = 0;
185  mesh.vertex_normals[1] = 0;
186  mesh.vertex_normals[2] = 1;
187 
188  mesh.vertex_normals[3] = 0;
189  mesh.vertex_normals[4] = 0;
190  mesh.vertex_normals[5] = 1;
191 
192  mesh.vertex_normals[6] = 0;
193  mesh.vertex_normals[7] = 0;
194  mesh.vertex_normals[8] = 1;
195 
196  mesh.vertex_normals[9] = 0;
197  mesh.vertex_normals[10] = 0;
198  mesh.vertex_normals[11] = 1;
199 
200  return mesh;
201 }
202 
203 template <typename Type>
204 bool MeshFilterTest<Type>::transform_callback(MeshHandle handle, Affine3d& transform) const
205 {
206  transform = Affine3d::Identity();
207  if (handle == handle_)
208  transform.translation() = Vector3d(0, 0, distance_);
209  return true;
210 }
211 
212 template <typename Type>
214 {
215  shapes::Mesh mesh = createMesh(0);
216  mesh_filter::MeshHandle handle = filter_.addMesh(mesh);
218 
219  vector<float> gt_depth(width_ * height_);
220  vector<unsigned int> gt_labels(width_ * height_);
221  getGroundTruth(&gt_labels[0], &gt_depth[0]);
222 
223  vector<float> filtered_depth(width_ * height_);
224  vector<unsigned int> filtered_labels(width_ * height_);
225  filter_.getFilteredDepth(&filtered_depth[0]);
226  filter_.getFilteredLabels(&filtered_labels[0]);
227 
228  for (unsigned idx = 0; idx < width_ * height_; ++idx)
229  {
230  // Only test if we are not very close to boundaries of object meshes and shadow-boundaries.
231  float sensor_depth = sensor_data_[idx] * FilterTraits<Type>::ToMetricScale;
232  if (fabs(sensor_depth - distance_ - shadow_) > epsilon_ && fabs(sensor_depth - distance_) > epsilon_)
233  {
234  ASSERT_FLOAT_EQ(filtered_depth[idx], gt_depth[idx]);
235  ASSERT_EQ(filtered_labels[idx], gt_labels[idx]);
236  }
237  }
238  filter_.removeMesh(handle);
239 }
240 
241 template <typename Type>
242 void MeshFilterTest<Type>::getGroundTruth(unsigned int* labels, float* depth) const
243 {
244  const double scale = FilterTraits<Type>::ToMetricScale;
245  if (distance_ <= near_ || distance_ >= far_)
246  {
247  // no filtering is done -> no shadow values or label values
248  for (unsigned yIdx = 0, idx = 0; yIdx < height_; ++yIdx)
249  {
250  for (unsigned xIdx = 0; xIdx < width_; ++xIdx, ++idx)
251  {
252  depth[idx] = double(sensor_data_[idx]) * scale;
253  if (depth[idx] < near_)
254  labels[idx] = MeshFilterBase::NearClip;
255  else if (depth[idx] >= far_)
256  labels[idx] = MeshFilterBase::FarClip;
257  else
258  labels[idx] = MeshFilterBase::Background;
259 
260  if (depth[idx] <= near_ || depth[idx] >= far_)
261  depth[idx] = 0;
262  }
263  }
264  }
265  else
266  {
267  for (unsigned yIdx = 0, idx = 0; yIdx < height_; ++yIdx)
268  {
269  for (unsigned xIdx = 0; xIdx < width_; ++xIdx, ++idx)
270  {
271  depth[idx] = double(sensor_data_[idx]) * scale;
272 
273  if (depth[idx] < near_)
274  {
275  labels[idx] = MeshFilterBase::NearClip;
276  depth[idx] = 0;
277  }
278  else
279  {
280  double diff = depth[idx] - distance_;
281  if (diff < 0 && depth[idx] < far_)
282  labels[idx] = MeshFilterBase::Background;
283  else if (diff > shadow_)
284  labels[idx] = MeshFilterBase::Shadow;
285  else if (depth[idx] >= far_)
286  labels[idx] = MeshFilterBase::FarClip;
287  else
288  {
289  labels[idx] = MeshFilterBase::FirstLabel;
290  depth[idx] = 0;
291  }
292 
293  if (depth[idx] >= far_)
294  depth[idx] = 0;
295  }
296  }
297  }
298  }
299 }
300 
301 } // namespace mesh_filter_test
302 
305 {
306  this->setMeshDistance(this->GetParam());
307  this->test();
308 }
309 INSTANTIATE_TEST_CASE_P(float_test, MeshFilterTestFloat, ::testing::Range<double>(0.0f, 6.0f, 0.5f));
310 
313 {
314  this->setMeshDistance(this->GetParam());
315  this->test();
316 }
317 INSTANTIATE_TEST_CASE_P(ushort_test, MeshFilterTestUnsignedShort, ::testing::Range<double>(0.0f, 6.0f, 0.5f));
318 
319 int main(int argc, char** argv)
320 {
321  testing::InitGoogleTest(&argc, argv);
322  int arg;
323 
324  return RUN_ALL_TESTS();
325 }
void setPaddingScale(float scale)
set the scale component of padding used to multiply with sensor-specific padding coefficients to get ...
Type
f
mesh_filter_test::MeshFilterTest< float > MeshFilterTestFloat
mesh_filter_test::MeshFilterTest< unsigned short > MeshFilterTestUnsignedShort
MeshFilter< StereoCameraModel > filter_
void getFilteredDepth(float *depth) const
retrieves the filtered depth values
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
int main(int argc, char **argv)
void getGroundTruth(unsigned int *labels, float *depth) const
void setMeshDistance(double distance)
MeshHandle addMesh(const shapes::Mesh &mesh)
adds a mesh to the filter object.
unsigned int * triangles
const Type getRandomNumber(const Type &min, const Type &max)
double * vertex_normals
void removeMesh(MeshHandle mesh_handle)
removes a mesh given by its handle
INSTANTIATE_TEST_CASE_P(float_test, MeshFilterTestFloat,::testing::Range< double >(0.0f, 6.0f, 0.5f))
Parameters for Stereo-like devices.
bool transform_callback(MeshHandle handle, Affine3d &transform) const
double epsilon
double z
void getFilteredLabels(LabelType *labels) const
retrieves the labels of the input data
double * vertices
StereoCameraModel::Parameters sensor_parameters_
TEST_P(MeshFilterTestFloat, float)
void setPaddingOffset(float offset)
set the offset component of padding. This value is added to the scaled sensor-specific constant compo...
void setShadowThreshold(float threshold)
set the shadow threshold. points that are further away than the rendered model are filtered out...
shapes::Mesh createMesh(double z) const
void filter(const void *sensor_data, GLushort type, bool wait=false) const
label/remove pixels from input depth-image
unsigned int MeshHandle


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Wed Jul 18 2018 02:48:55