41 #include <Eigen/Eigen> 47 #ifdef HAVE_SSE_EXTENSIONS 48 #include <xmmintrin.h> 52 using namespace Eigen;
56 : mesh_renderer_(640, 480, 0.3, 10)
57 , depth_filter_(640, 480, 0.3, 10)
58 , next_handle_(FirstLabel)
59 , transform_callback_(transform_callback)
60 , shadow_threshold_(0.5)
62 mesh_renderer_.setShadersFromString(padding_vertex_shader_,
"");
63 depth_filter_.setShadersFromString(filter_vertex_shader_, filter_fragment_shader_);
65 depth_filter_.begin();
67 glGenTextures(1, &sensor_depth_texture_);
69 glUniform1i(glGetUniformLocation(depth_filter_.getProgramID(),
"sensor"), 0);
70 glUniform1i(glGetUniformLocation(depth_filter_.getProgramID(),
"depth"), 2);
71 glUniform1i(glGetUniformLocation(depth_filter_.getProgramID(),
"label"), 4);
73 near_location_ = glGetUniformLocation(depth_filter_.getProgramID(),
"near");
74 far_location_ = glGetUniformLocation(depth_filter_.getProgramID(),
"far");
75 shadow_threshold_location_ = glGetUniformLocation(depth_filter_.getProgramID(),
"shadow_threshold");
79 canvas_ = glGenLists(1);
80 glNewList(canvas_, GL_COMPILE);
85 glVertex3f(-1, -1, 0);
100 mesh_filter::MeshFilter::~MeshFilter()
102 glDeleteLists(1, canvas_);
103 glDeleteTextures(1, &sensor_depth_texture_);
105 for (map<unsigned int, GLMesh*>::iterator meshIt = meshes_.begin(); meshIt != meshes_.end(); ++meshIt)
106 delete (meshIt->second);
112 mesh_renderer_.setBufferSize(width, height);
113 mesh_renderer_.setCameraParameters(width, width, width >> 1, height >> 1);
115 depth_filter_.setBufferSize(width, height);
116 depth_filter_.setCameraParameters(width, width, width >> 1, height >> 1);
122 transform_callback_ = transform_callback;
125 void mesh_filter::MeshFilter::setPaddingCoefficients(
const Eigen::Vector3f& padding_coefficients)
127 padding_coefficients_ = padding_coefficients;
132 Mesh collapsedMesh(1, 1);
133 mergeVertices(mesh, collapsedMesh);
134 meshes_[next_handle_] =
new GLMesh(collapsedMesh, next_handle_);
135 return next_handle_++;
140 long unsigned int erased = meshes_.erase(handle);
142 throw runtime_error(
"Could not remove mesh. Mesh not found!");
147 shadow_threshold_ = threshold;
152 mesh_renderer_.getColorBuffer((
unsigned char*)labels);
157 inline unsigned alignment16(
const void* pointer)
159 return ((uintptr_t)pointer & 15);
161 inline bool isAligned16(
const void* pointer)
163 return (((uintptr_t)pointer & 15) == 0);
169 mesh_renderer_.getDepthBuffer(depth);
170 #if HAVE_SSE_EXTENSIONS 171 const __m128 mmNear = _mm_set1_ps(mesh_renderer_.getNearClippingDistance());
172 const __m128 mmFar = _mm_set1_ps(mesh_renderer_.getFarClippingDistance());
173 const __m128 mmNF = _mm_mul_ps(mmNear, mmFar);
174 const __m128 mmF_N = _mm_sub_ps(mmFar, mmNear);
175 static const __m128 mmOnes = _mm_set1_ps(1);
176 static const __m128 mmZeros = _mm_set1_ps(0);
178 float* depthEnd = depth + mesh_renderer_.getHeight() * mesh_renderer_.getWidth();
179 if (!isAligned16(depth))
182 unsigned first = 16 - alignment16(depth);
184 const float near = mesh_renderer_.getNearClippingDistance();
185 const float far = mesh_renderer_.getFarClippingDistance();
186 const float nf = near * far;
187 const float f_n = far - near;
189 while (depth < depthEnd && idx++ < first)
190 if (*depth != 0 && *depth != 1)
191 *depth = nf / (far - *depth * f_n);
196 unsigned last = (depth_filter_.getWidth() * depth_filter_.getHeight() - first) & 15;
197 float* depth2 = depthEnd - last;
198 while (depth2 < depthEnd)
199 if (*depth2 != 0 && *depth2 != 1)
200 *depth2 = nf / (far - *depth2 * f_n);
207 const __m128* mmEnd = (__m128*)depthEnd;
208 __m128* mmDepth = (__m128*)depth;
210 while (mmDepth < mmEnd)
212 __m128 mask = _mm_and_ps(_mm_cmpneq_ps(*mmDepth, mmOnes), _mm_cmpneq_ps(*mmDepth, mmZeros));
213 *mmDepth = _mm_mul_ps(*mmDepth, mmF_N);
214 *mmDepth = _mm_sub_ps(mmFar, *mmDepth);
215 *mmDepth = _mm_div_ps(mmNF, *mmDepth);
216 *mmDepth = _mm_and_ps(*mmDepth, mask);
222 const float near = mesh_renderer_.getNearClippingDistance();
223 const float far = mesh_renderer_.getFarClippingDistance();
224 const float nf = near * far;
225 const float f_n = far - near;
227 const float* depthEnd = depth + mesh_renderer_.getHeight() * mesh_renderer_.getWidth();
228 while (depth < depthEnd)
230 if (*depth != 0 && *depth != 1)
231 *depth = nf / (far - *depth * f_n);
242 depth_filter_.getDepthBuffer(depth);
243 #if HAVE_SSE_EXTENSIONS 245 const __m128 mmNear = _mm_set1_ps(depth_filter_.getNearClippingDistance());
246 const __m128 mmFar = _mm_set1_ps(depth_filter_.getFarClippingDistance());
247 const __m128 mmScale = _mm_sub_ps(mmFar, mmNear);
248 float* depthEnd = depth + depth_filter_.getWidth() * depth_filter_.getHeight();
250 if (!isAligned16(depth))
253 unsigned first = 16 - alignment16(depth);
255 const float scale = depth_filter_.getFarClippingDistance() - depth_filter_.getNearClippingDistance();
256 const float offset = depth_filter_.getNearClippingDistance();
257 while (depth < depthEnd && idx++ < first)
258 if (*depth != 0 && *depth != 1.0)
259 *depth = *depth * scale + offset;
264 unsigned last = (depth_filter_.getWidth() * depth_filter_.getHeight() - first) & 15;
265 float* depth2 = depthEnd - last;
266 while (depth2 < depthEnd)
267 if (*depth2 != 0 && *depth != 1.0)
268 *depth2 = *depth2 * scale + offset;
275 const __m128* mmEnd = (__m128*)depthEnd;
276 __m128* mmDepth = (__m128*)depth;
278 while (mmDepth < mmEnd)
280 *mmDepth = _mm_mul_ps(*mmDepth, mmScale);
281 *mmDepth = _mm_add_ps(*mmDepth, mmNear);
282 *mmDepth = _mm_and_ps(*mmDepth, _mm_and_ps(_mm_cmpneq_ps(*mmDepth, mmNear), _mm_cmpneq_ps(*mmDepth, mmFar)));
286 const float* depthEnd = depth + depth_filter_.getWidth() * depth_filter_.getHeight();
287 const float scale = depth_filter_.getFarClippingDistance() - depth_filter_.getNearClippingDistance();
288 const float offset = depth_filter_.getNearClippingDistance();
289 while (depth < depthEnd)
293 if (*depth != 0 && *depth != 1.0)
294 *depth = *depth * scale + offset;
305 depth_filter_.getColorBuffer((
unsigned char*)labels);
309 float cx,
float cy)
const 311 mesh_renderer_.setBufferSize(width, height);
312 mesh_renderer_.setCameraParameters(fx, fy, cx, cy);
313 mesh_renderer_.begin();
314 glEnable(GL_DEPTH_TEST);
315 glEnable(GL_CULL_FACE);
316 glCullFace(GL_FRONT);
317 glDisable(GL_ALPHA_TEST);
319 GLuint padding_coefficients_id = glGetUniformLocation(mesh_renderer_.getProgramID(),
"padding_coefficients");
320 glUniform3f(padding_coefficients_id, padding_coefficients_[0], padding_coefficients_[1], padding_coefficients_[2]);
323 for (map<unsigned int, GLMesh*>::const_iterator meshIt = meshes_.begin(); meshIt != meshes_.end(); ++meshIt)
324 if (transform_callback_(meshIt->first, transform))
325 meshIt->second->render(transform);
327 mesh_renderer_.end();
330 depth_filter_.setBufferSize(width, height);
331 depth_filter_.setCameraParameters(fx, fy, cx, cy);
332 depth_filter_.begin();
333 glEnable(GL_DEPTH_TEST);
334 glEnable(GL_CULL_FACE);
336 glDisable(GL_ALPHA_TEST);
338 glUniform1f(near_location_, depth_filter_.getNearClippingDistance());
339 glUniform1f(far_location_, depth_filter_.getFarClippingDistance());
340 glUniform1f(shadow_threshold_location_, shadow_threshold_);
342 GLuint depth_texture = mesh_renderer_.getDepthTexture();
343 GLuint color_texture = mesh_renderer_.getColorTexture();
346 glActiveTexture(GL_TEXTURE0);
347 glBindTexture(GL_TEXTURE_2D, sensor_depth_texture_);
349 float scale = 1.0 / (depth_filter_.getFarClippingDistance() - depth_filter_.getNearClippingDistance());
350 glPixelTransferf(GL_DEPTH_SCALE, scale);
351 glPixelTransferf(GL_DEPTH_BIAS, -scale * depth_filter_.getNearClippingDistance());
352 glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, width, height, 0, GL_DEPTH_COMPONENT, GL_FLOAT, sensor_data);
353 glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
354 glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
355 glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
356 glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
359 glActiveTexture(GL_TEXTURE2);
360 glBindTexture(GL_TEXTURE_2D, depth_texture);
363 glActiveTexture(GL_TEXTURE4);
364 glBindTexture(GL_TEXTURE_2D, color_texture);
367 glDisable(GL_TEXTURE_2D);
371 void mesh_filter::MeshFilter::setClippingRange(
float near,
float far)
373 mesh_renderer_.setClippingRange(near, far);
374 depth_filter_.setClippingRange(near, far);
377 void mesh_filter::MeshFilter::mergeVertices(
const Mesh& mesh,
Mesh& compressed)
381 static const double thresholdSQR =
pow(0.00001, 2);
385 vector<Vector3d> compressed_vertices;
388 for (
unsigned vIdx = 0; vIdx < mesh.
vertex_count; ++vIdx)
390 vertices[vIdx][0] = mesh.
vertices[3 * vIdx];
391 vertices[vIdx][1] = mesh.
vertices[3 * vIdx + 1];
392 vertices[vIdx][2] = mesh.
vertices[3 * vIdx + 2];
393 vertex_map[vIdx] = vIdx;
398 triangles[tIdx][0] = mesh.
triangles[3 * tIdx];
399 triangles[tIdx][1] = mesh.
triangles[3 * tIdx + 1];
400 triangles[tIdx][2] = mesh.
triangles[3 * tIdx + 2];
403 for (
unsigned vIdx1 = 0; vIdx1 < mesh.
vertex_count; ++vIdx1)
405 if (vertex_map[vIdx1] != vIdx1)
408 vertex_map[vIdx1] = compressed_vertices.size();
409 compressed_vertices.push_back(vertices[vIdx1]);
411 for (
unsigned vIdx2 = vIdx1 + 1; vIdx2 < mesh.
vertex_count; ++vIdx2)
413 double distanceSQR = (vertices[vIdx1] - vertices[vIdx2]).squaredNorm();
414 if (distanceSQR <= thresholdSQR)
415 vertex_map[vIdx2] = vertex_map[vIdx1];
422 triangles[tIdx][0] = vertex_map[triangles[tIdx][0]];
423 triangles[tIdx][1] = vertex_map[triangles[tIdx][1]];
424 triangles[tIdx][2] = vertex_map[triangles[tIdx][2]];
427 for (
int vIdx = 0; vIdx < vertices.size(); ++vIdx)
429 if (vertices[vIdx][0] == vertices[vIdx][1] || vertices[vIdx][0] == vertices[vIdx][2] ||
430 vertices[vIdx][1] == vertices[vIdx][2])
432 vertices[vIdx] = vertices.back();
444 delete[] compressed.normals;
448 for (
unsigned vIdx = 0; vIdx < compressed.
vertex_count; ++vIdx)
450 compressed.
vertices[3 * vIdx + 0] = compressed_vertices[vIdx][0];
451 compressed.
vertices[3 * vIdx + 1] = compressed_vertices[vIdx][1];
452 compressed.
vertices[3 * vIdx + 2] = compressed_vertices[vIdx][2];
459 compressed.
triangles[3 * tIdx + 0] = triangles[tIdx][0];
460 compressed.
triangles[3 * tIdx + 1] = triangles[tIdx][1];
461 compressed.
triangles[3 * tIdx + 2] = triangles[tIdx][2];
467 Vector3d d1 = compressed_vertices[triangles[tIdx][1]] - compressed_vertices[triangles[tIdx][0]];
468 Vector3d
d2 = compressed_vertices[triangles[tIdx][2]] - compressed_vertices[triangles[tIdx][0]];
469 Vector3d normal = d1.cross(d2);
472 normal_[0] = mesh.normals[3 * tIdx];
473 normal_[1] = mesh.normals[3 * tIdx + 1];
474 normal_[2] = mesh.normals[3 * tIdx + 2];
475 double cosAngle = normal.dot(normal_);
483 compressed.normals[tIdx * 3 + 0] = normal[0];
484 compressed.normals[tIdx * 3 + 1] = normal[1];
485 compressed.normals[tIdx * 3 + 2] = normal[2];
489 string mesh_filter::MeshFilter::padding_vertex_shader_ =
491 "uniform vec3 padding_coefficients;" 493 "{gl_FrontColor = gl_Color;" 494 "vec4 vertex = gl_ModelViewMatrix * gl_Vertex;" 495 "vec3 normal = normalize(gl_NormalMatrix * gl_Normal);" 496 "float lambda = padding_coefficients.x * vertex.z * vertex.z + padding_coefficients.y * vertex.z + " 497 "padding_coefficients.z;" 498 "gl_Position = gl_ProjectionMatrix * (vertex + lambda * vec4(normal,0) );" 499 "gl_Position.y = -gl_Position.y;}";
501 string mesh_filter::MeshFilter::filter_vertex_shader_ =
"#version 120\n" 504 " gl_FrontColor = gl_Color;" 505 " gl_TexCoord[0] = gl_MultiTexCoord0;" 506 " gl_Position = gl_Vertex;" 507 " gl_Position.w = 1.0;" 510 string mesh_filter::MeshFilter::filter_fragment_shader_ =
512 "uniform sampler2D sensor;" 513 "uniform sampler2D depth;" 514 "uniform sampler2D label;" 515 "uniform float near;" 517 "uniform float shadow_threshold;" 518 "float f_n = far - near;" 519 "float threshold = shadow_threshold / f_n;" 522 " float dValue = float(texture2D(depth, gl_TexCoord[0].st));" 523 " float zValue = dValue * near / (far - dValue * f_n);" 524 " float diff = float(texture2D(sensor, gl_TexCoord[0].st)) - zValue;" 526 " gl_FragColor = vec4 (0, 0, 0, 0);" 527 " gl_FragDepth = float(texture2D(sensor, gl_TexCoord[0].st));" 528 " } else if (diff > threshold) {" 529 " gl_FragColor = vec4 (0.003921569, 0, 0, 0);" 530 " gl_FragDepth = float(texture2D(sensor, gl_TexCoord[0].st));" 532 " gl_FragColor = vec4 (1,1,1,1);"
void setTransformCallback(const TransformCallback &transform_callback)
set the callback for retrieving transformations for each mesh.
MeshFilter(const TransformCallback &transform_callback=TransformCallback(), const typename SensorType::Parameters &sensor_parameters=typename SensorType::Parameters())
Constructor.
void getFilteredDepth(float *depth) const
retrieves the filtered depth values
MeshHandle addMesh(const shapes::Mesh &mesh)
adds a mesh to the filter object.
unsigned int vertex_count
void removeMesh(MeshHandle mesh_handle)
removes a mesh given by its handle
void swap(ChunkedFile &a, ChunkedFile &b)
unsigned int triangle_count
void getFilteredLabels(LabelType *labels) const
retrieves the labels of the input data
void getModelDepth(float *depth) const
retrieves the depth values of the rendered model
void getModelLabels(LabelType *labels) const
retrieves the labels of the rendered model
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
void setSize(unsigned int width, unsigned int height)
sets the size of the fram buffers
void setShadowThreshold(float threshold)
set the shadow threshold. points that are further away than the rendered model are filtered out...
void filter(const void *sensor_data, GLushort type, bool wait=false) const
label/remove pixels from input depth-image