moveit2
The MoveIt Motion Planning Framework for ROS 2.
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>
38 #include <geometric_shapes/shapes.h>
39 #include <geometric_shapes/shape_operations.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 std::placeholders;
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  static_assert(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();
90  {
91  distance_ = distance;
92  }
93 
94 private:
95  shapes::Mesh createMesh(double z) const;
96  bool transformCallback(MeshHandle handle, Isometry3d& 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_;
104  StereoCameraModel::Parameters sensor_parameters_;
106  MeshHandle handle_;
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_([this](mesh_filter::MeshHandle mesh, Eigen::Isometry3d& tf) { return transformCallback(mesh, tf); },
122  sensor_parameters_)
123  , sensor_data_(width_ * height_)
124  , distance_(0.0)
125 {
126  filter_.setShadowThreshold(shadow_);
127  // no padding
128  filter_.setPaddingOffset(0.0);
129  filter_.setPaddingScale(0.0);
130 
131  // create a large plane that covers the whole visible area -> no boundaries
132 
133  shapes::Mesh mesh = createMesh(0);
134  handle_ = filter_.addMesh(mesh);
135 
136  // make it random but reproducible
137  srand(0);
138  Type t_near = near_ / FilterTraits<Type>::ToMetricScale;
140  for (typename vector<Type>::iterator s_it = sensor_data_.begin(); s_it != sensor_data_.end(); ++s_it)
141  {
142  do
143  {
144  *s_it = getRandomNumber<Type>(0.0, 10.0 / FilterTraits<Type>::ToMetricScale);
145  } while (*s_it == t_near || *s_it == t_far);
146  }
147 }
148 
149 template <typename Type>
150 shapes::Mesh MeshFilterTest<Type>::createMesh(double z) const
151 {
152  shapes::Mesh mesh(4, 4);
153  mesh.vertices[0] = -5;
154  mesh.vertices[1] = -5;
155  mesh.vertices[2] = z;
156 
157  mesh.vertices[3] = -5;
158  mesh.vertices[4] = 5;
159  mesh.vertices[5] = z;
160 
161  mesh.vertices[6] = 5;
162  mesh.vertices[7] = 5;
163  mesh.vertices[8] = z;
164 
165  mesh.vertices[9] = 5;
166  mesh.vertices[10] = -5;
167  mesh.vertices[11] = z;
168 
169  mesh.triangles[0] = 0;
170  mesh.triangles[1] = 3;
171  mesh.triangles[2] = 2;
172 
173  mesh.triangles[3] = 0;
174  mesh.triangles[4] = 2;
175  mesh.triangles[5] = 1;
176 
177  mesh.triangles[6] = 0;
178  mesh.triangles[7] = 2;
179  mesh.triangles[8] = 3;
180 
181  mesh.triangles[9] = 0;
182  mesh.triangles[10] = 1;
183  mesh.triangles[11] = 2;
184 
185  mesh.vertex_normals[0] = 0;
186  mesh.vertex_normals[1] = 0;
187  mesh.vertex_normals[2] = 1;
188 
189  mesh.vertex_normals[3] = 0;
190  mesh.vertex_normals[4] = 0;
191  mesh.vertex_normals[5] = 1;
192 
193  mesh.vertex_normals[6] = 0;
194  mesh.vertex_normals[7] = 0;
195  mesh.vertex_normals[8] = 1;
196 
197  mesh.vertex_normals[9] = 0;
198  mesh.vertex_normals[10] = 0;
199  mesh.vertex_normals[11] = 1;
200 
201  return mesh;
202 }
203 
204 template <typename Type>
205 bool MeshFilterTest<Type>::transformCallback(MeshHandle handle, Isometry3d& transform) const
206 {
207  transform = Isometry3d::Identity();
208  if (handle == handle_)
209  transform.translation() = Vector3d(0, 0, distance_);
210  return true;
211 }
212 
213 template <typename Type>
215 {
216  shapes::Mesh mesh = createMesh(0);
217  mesh_filter::MeshHandle handle = filter_.addMesh(mesh);
218  filter_.filter(&sensor_data_[0], FilterTraits<Type>::FILTER_GL_TYPE, false);
219 
220  vector<float> gt_depth(width_ * height_);
221  vector<unsigned int> gt_labels(width_ * height_);
222  getGroundTruth(&gt_labels[0], &gt_depth[0]);
223 
224  vector<float> filtered_depth(width_ * height_);
225  vector<unsigned int> filtered_labels(width_ * height_);
226  filter_.getFilteredDepth(&filtered_depth[0]);
227  filter_.getFilteredLabels(&filtered_labels[0]);
228 
229  for (unsigned idx = 0; idx < width_ * height_; ++idx)
230  {
231  // Only test if we are not very close to boundaries of object meshes and shadow-boundaries.
232  float sensor_depth = sensor_data_[idx] * FilterTraits<Type>::ToMetricScale;
233  if (fabs(sensor_depth - distance_ - shadow_) > epsilon_ && fabs(sensor_depth - distance_) > epsilon_)
234  {
235  ASSERT_NEAR(filtered_depth[idx], gt_depth[idx], 1e-4);
236  ASSERT_EQ(filtered_labels[idx], gt_labels[idx]);
237  }
238  }
239  filter_.removeMesh(handle);
240 }
241 
242 template <typename Type>
243 void MeshFilterTest<Type>::getGroundTruth(unsigned int* labels, float* depth) const
244 {
245  const double scale = FilterTraits<Type>::ToMetricScale;
246  if (distance_ <= near_ || distance_ >= far_)
247  {
248  // no filtering is done -> no shadow values or label values
249  for (unsigned y_idx = 0, idx = 0; y_idx < height_; ++y_idx)
250  {
251  for (unsigned x_idx = 0; x_idx < width_; ++x_idx, ++idx)
252  {
253  depth[idx] = double(sensor_data_[idx]) * scale;
254  if (depth[idx] < near_)
255  labels[idx] = MeshFilterBase::NEAR_CLIP;
256  else if (depth[idx] >= far_)
257  labels[idx] = MeshFilterBase::FAR_CLIP;
258  else
259  labels[idx] = MeshFilterBase::BACKGROUND;
260 
261  if (depth[idx] <= near_ || depth[idx] >= far_)
262  depth[idx] = 0;
263  }
264  }
265  }
266  else
267  {
268  for (unsigned y_idx = 0, idx = 0; y_idx < height_; ++y_idx)
269  {
270  for (unsigned x_idx = 0; x_idx < width_; ++x_idx, ++idx)
271  {
272  depth[idx] = double(sensor_data_[idx]) * scale;
273 
274  if (depth[idx] < near_)
275  {
276  labels[idx] = MeshFilterBase::NEAR_CLIP;
277  depth[idx] = 0;
278  }
279  else
280  {
281  double diff = depth[idx] - distance_;
282  if (diff < 0 && depth[idx] < far_)
283  labels[idx] = MeshFilterBase::BACKGROUND;
284  else if (diff > shadow_)
285  labels[idx] = MeshFilterBase::SHADOW;
286  else if (depth[idx] >= far_)
287  labels[idx] = MeshFilterBase::FAR_CLIP;
288  else
289  {
290  labels[idx] = MeshFilterBase::FIRST_LABEL;
291  depth[idx] = 0;
292  }
293 
294  if (depth[idx] >= far_)
295  depth[idx] = 0;
296  }
297  }
298  }
299  }
300 }
301 
302 } // namespace mesh_filter_test
303 
306 {
307  this->setMeshDistance(this->GetParam());
308  this->test();
309 }
310 INSTANTIATE_TEST_CASE_P(float_test, MeshFilterTestFloat, ::testing::Range<double>(0.0f, 6.0f, 0.5f));
311 
314 {
315  this->setMeshDistance(this->GetParam());
316  this->test();
317 }
318 INSTANTIATE_TEST_CASE_P(ushort_test, MeshFilterTestUnsignedShort, ::testing::Range<double>(0.0f, 6.0f, 0.5f));
319 
320 int main(int argc, char** argv)
321 {
322  testing::InitGoogleTest(&argc, argv);
323  return RUN_ALL_TESTS();
324 }
Parameters for Stereo-like devices.
void setMeshDistance(double distance)
int main(int argc, char **argv)
INSTANTIATE_TEST_CASE_P(float_test, MeshFilterTestFloat, ::testing::Range< double >(0.0f, 6.0f, 0.5f))
mesh_filter_test::MeshFilterTest< float > MeshFilterTestFloat
TEST_P(MeshFilterTestFloat, float)
mesh_filter_test::MeshFilterTest< unsigned short > MeshFilterTestUnsignedShort
Type
The types of bodies that are considered for collision.
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
const Type getRandomNumber(const Type &min, const Type &max)
unsigned int MeshHandle
z
Definition: pick.py:66
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55