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
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 swap(scoped_ptr< T > &, scoped_ptr< T > &)
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