35 #include <gtest/gtest.h>
40 #include <eigen3/Eigen/Eigen>
44 using namespace Eigen;
46 using namespace std::placeholders;
50 template <
typename Type>
53 return Type(
min + (max -
min) *
double(rand()) /
double(RAND_MAX));
56 template <
typename Type>
60 static const GLushort FILTER_GL_TYPE = GL_ZERO;
67 static const GLushort FILTER_GL_TYPE = GL_UNSIGNED_SHORT;
68 static constexpr
double ToMetricScale = 0.001;
75 static const GLushort FILTER_GL_TYPE = GL_FLOAT;
76 static constexpr
double ToMetricScale = 1.0f;
79 template <
typename Type>
86 MeshFilterTest(
unsigned width = 500,
unsigned height = 500,
double near = 0.5,
double far = 5.0,
double shadow = 0.1,
96 bool transformCallback(
MeshHandle handle, Isometry3d& transform)
const;
97 void getGroundTruth(
unsigned int* labels,
float* depth)
const;
111 template <
typename Type>
120 , sensor_parameters_(width, height, near_, far_, width >> 1, height >> 1, width >> 1, height >> 1, 0.1, 0.1)
123 , sensor_data_(width_ * height_)
126 filter_.setShadowThreshold(shadow_);
128 filter_.setPaddingOffset(0.0);
129 filter_.setPaddingScale(0.0);
134 handle_ = filter_.addMesh(mesh);
140 for (
typename vector<Type>::iterator s_it = sensor_data_.begin(); s_it != sensor_data_.end(); ++s_it)
145 }
while (*s_it == t_near || *s_it == t_far);
149 template <
typename Type>
204 template <
typename Type>
208 if (handle == handle_)
213 template <
typename Type>
220 vector<float> gt_depth(width_ * height_);
221 vector<unsigned int> gt_labels(width_ * height_);
222 getGroundTruth(>_labels[0], >_depth[0]);
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]);
229 for (
unsigned idx = 0; idx < width_ * height_; ++idx)
233 if (fabs(sensor_depth - distance_ - shadow_) > epsilon_ && fabs(sensor_depth - distance_) > epsilon_)
235 ASSERT_NEAR(filtered_depth[idx], gt_depth[idx], 1e-4);
236 ASSERT_EQ(filtered_labels[idx], gt_labels[idx]);
239 filter_.removeMesh(handle);
242 template <
typename Type>
246 if (distance_ <= near_ || distance_ >= far_)
249 for (
unsigned y_idx = 0, idx = 0; y_idx < height_; ++y_idx)
251 for (
unsigned x_idx = 0; x_idx < width_; ++x_idx, ++idx)
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;
259 labels[idx] = MeshFilterBase::BACKGROUND;
261 if (depth[idx] <= near_ || depth[idx] >= far_)
268 for (
unsigned y_idx = 0, idx = 0; y_idx < height_; ++y_idx)
270 for (
unsigned x_idx = 0; x_idx < width_; ++x_idx, ++idx)
272 depth[idx] = double(sensor_data_[idx]) * scale;
274 if (depth[idx] < near_)
276 labels[idx] = MeshFilterBase::NEAR_CLIP;
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;
290 labels[idx] = MeshFilterBase::FIRST_LABEL;
294 if (depth[idx] >= far_)
307 this->setMeshDistance(this->GetParam());
315 this->setMeshDistance(this->GetParam());
320 int main(
int argc,
char** argv)
322 testing::InitGoogleTest(&argc, argv);
323 return RUN_ALL_TESTS();