00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/mesh_filter/mesh_filter.h>
00038 #include <moveit/mesh_filter/gl_mesh.h>
00039 #include <geometric_shapes/shapes.h>
00040 #include <geometric_shapes/shape_operations.h>
00041 #include <Eigen/Eigen>
00042 #include <stdexcept>
00043 #include <sensor_msgs/image_encodings.h>
00044 #include <stdint.h>
00045
00046
00047 #ifdef HAVE_SSE_EXTENSIONS
00048 #include <xmmintrin.h>
00049 #endif
00050
00051 using namespace std;
00052 using namespace Eigen;
00053 using shapes::Mesh;
00054
00055 mesh_filter::MeshFilter::MeshFilter (const boost::function<bool(MeshFilter::MeshHandle, Affine3d&)>& transform_callback)
00056 : mesh_renderer_ (640, 480, 0.3, 10)
00057 , depth_filter_ (640, 480, 0.3, 10)
00058 , next_handle_ (FirstLabel)
00059 , transform_callback_ (transform_callback)
00060 , shadow_threshold_ (0.5)
00061 {
00062 mesh_renderer_.setShadersFromString (padding_vertex_shader_, "");
00063 depth_filter_.setShadersFromString (filter_vertex_shader_, filter_fragment_shader_);
00064
00065 depth_filter_.begin ();
00066
00067 glGenTextures (1, &sensor_depth_texture_);
00068
00069 glUniform1i (glGetUniformLocation (depth_filter_.getProgramID (), "sensor"), 0);
00070 glUniform1i (glGetUniformLocation (depth_filter_.getProgramID (), "depth"), 2);
00071 glUniform1i (glGetUniformLocation (depth_filter_.getProgramID (), "label"), 4);
00072
00073 near_location_ = glGetUniformLocation (depth_filter_.getProgramID (), "near");
00074 far_location_ = glGetUniformLocation (depth_filter_.getProgramID (), "far");
00075 shadow_threshold_location_ = glGetUniformLocation (depth_filter_.getProgramID (), "shadow_threshold");
00076
00077 depth_filter_.end ();
00078
00079 canvas_ = glGenLists (1);
00080 glNewList (canvas_, GL_COMPILE);
00081 glBegin (GL_QUADS);
00082
00083 glColor3f (1, 1, 1);
00084 glTexCoord2f (0, 0);
00085 glVertex3f (-1, -1, 0);
00086
00087 glTexCoord2f (1, 0);
00088 glVertex3f (1, -1, 0);
00089
00090 glTexCoord2f ( 1, 1);
00091 glVertex3f (1, 1, 0);
00092
00093 glTexCoord2f ( 0, 1);
00094 glVertex3f (-1, 1, 0);
00095
00096 glEnd ();
00097 glEndList ();
00098 }
00099
00100 mesh_filter::MeshFilter::~MeshFilter ()
00101 {
00102 glDeleteLists (1, canvas_);
00103 glDeleteTextures (1, &sensor_depth_texture_);
00104
00105 for (map<unsigned int, GLMesh*>::iterator meshIt = meshes_.begin (); meshIt != meshes_.end (); ++meshIt)
00106 delete (meshIt->second);
00107 meshes_.clear ();
00108 }
00109
00110 void mesh_filter::MeshFilter::setSize (unsigned width, unsigned height)
00111 {
00112 mesh_renderer_.setBufferSize (width, height);
00113 mesh_renderer_.setCameraParameters (width, width, width >> 1, height >> 1);
00114
00115 depth_filter_.setBufferSize (width, height);
00116 depth_filter_.setCameraParameters (width, width, width >> 1, height >> 1);
00117 }
00118
00119 void mesh_filter::MeshFilter::setTransformCallback (const boost::function<bool(mesh_filter::MeshFilter::MeshHandle, Affine3d&)>& transform_callback)
00120 {
00121 transform_callback_ = transform_callback;
00122 }
00123
00124 void mesh_filter::MeshFilter::setPaddingCoefficients (const Eigen::Vector3f& padding_coefficients)
00125 {
00126 padding_coefficients_ = padding_coefficients;
00127 }
00128
00129 mesh_filter::MeshFilter::MeshHandle mesh_filter::MeshFilter::addMesh (const Mesh& mesh)
00130 {
00131 Mesh collapsedMesh (1, 1);
00132 mergeVertices (mesh, collapsedMesh);
00133 meshes_[next_handle_] = new GLMesh (collapsedMesh, next_handle_);
00134 return next_handle_++;
00135 }
00136
00137 void mesh_filter::MeshFilter::removeMesh (MeshFilter::MeshHandle handle)
00138 {
00139 long unsigned int erased = meshes_.erase (handle);
00140 if (erased == 0)
00141 throw runtime_error ("Could not remove mesh. Mesh not found!");
00142 }
00143
00144 void mesh_filter::MeshFilter::setShadowThreshold (float threshold)
00145 {
00146 shadow_threshold_ = threshold;
00147 }
00148
00149 void mesh_filter::MeshFilter::getModelLabels (unsigned* labels) const
00150 {
00151 mesh_renderer_.getColorBuffer ((unsigned char*) labels);
00152 }
00153
00154 namespace
00155 {
00156 inline unsigned alignment16 (const void * pointer) { return ((uintptr_t)pointer & 15); }
00157 inline bool isAligned16 (const void* pointer) { return (((uintptr_t)pointer & 15) == 0); }
00158 }
00159
00160 void mesh_filter::MeshFilter::getModelDepth (float* depth) const
00161 {
00162 mesh_renderer_.getDepthBuffer (depth);
00163 #if HAVE_SSE_EXTENSIONS
00164 const __m128 mmNear = _mm_set1_ps (mesh_renderer_.getNearClippingDistance ());
00165 const __m128 mmFar = _mm_set1_ps (mesh_renderer_.getFarClippingDistance ());
00166 const __m128 mmNF = _mm_mul_ps (mmNear, mmFar);
00167 const __m128 mmF_N = _mm_sub_ps (mmFar, mmNear);
00168 static const __m128 mmOnes = _mm_set1_ps (1);
00169 static const __m128 mmZeros = _mm_set1_ps (0);
00170
00171 float* depthEnd = depth + mesh_renderer_.getHeight () * mesh_renderer_.getWidth ();
00172 if (!isAligned16 (depth))
00173 {
00174
00175 unsigned first = 16 - alignment16 (depth);
00176 unsigned idx;
00177 const float near = mesh_renderer_.getNearClippingDistance ();
00178 const float far = mesh_renderer_.getFarClippingDistance ();
00179 const float nf = near * far;
00180 const float f_n = far - near;
00181
00182 while (depth < depthEnd && idx++ < first)
00183 if (*depth != 0 && *depth != 1)
00184 *depth = nf / (far - *depth * f_n);
00185 else
00186 *depth = 0;
00187
00188
00189 unsigned last = (depth_filter_.getWidth () * depth_filter_.getHeight () - first) & 15;
00190 float* depth2 = depthEnd - last;
00191 while (depth2 < depthEnd)
00192 if (*depth2 != 0 && *depth2 != 1)
00193 *depth2 = nf / (far - *depth2 * f_n);
00194 else
00195 *depth2 = 0;
00196
00197 depthEnd -= last;
00198 }
00199
00200 const __m128* mmEnd = (__m128*) depthEnd;
00201 __m128* mmDepth = (__m128*) depth;
00202
00203 while (mmDepth < mmEnd)
00204 {
00205 __m128 mask = _mm_and_ps (_mm_cmpneq_ps (*mmDepth, mmOnes), _mm_cmpneq_ps (*mmDepth, mmZeros));
00206 *mmDepth = _mm_mul_ps (*mmDepth, mmF_N);
00207 *mmDepth = _mm_sub_ps (mmFar, *mmDepth);
00208 *mmDepth = _mm_div_ps (mmNF, *mmDepth);
00209 *mmDepth = _mm_and_ps (*mmDepth, mask);
00210 ++mmDepth;
00211 }
00212
00213 #else
00214
00215 const float near = mesh_renderer_.getNearClippingDistance ();
00216 const float far = mesh_renderer_.getFarClippingDistance ();
00217 const float nf = near * far;
00218 const float f_n = far - near;
00219
00220 const float* depthEnd = depth + mesh_renderer_.getHeight () * mesh_renderer_.getWidth ();
00221 while (depth < depthEnd)
00222 {
00223 if (*depth != 0 && *depth != 1)
00224 *depth = nf / (far - *depth * f_n);
00225 else
00226 *depth = 0;
00227
00228 ++depth;
00229 }
00230 #endif
00231 }
00232
00233 void mesh_filter::MeshFilter::getFilteredDepth (float* depth) const
00234 {
00235 depth_filter_.getDepthBuffer (depth);
00236 #if HAVE_SSE_EXTENSIONS
00237
00238 const __m128 mmNear = _mm_set1_ps (depth_filter_.getNearClippingDistance ());
00239 const __m128 mmFar = _mm_set1_ps (depth_filter_.getFarClippingDistance ());
00240 const __m128 mmScale = _mm_sub_ps (mmFar, mmNear);
00241 float *depthEnd = depth + depth_filter_.getWidth () * depth_filter_.getHeight ();
00242
00243 if (!isAligned16 (depth))
00244 {
00245
00246 unsigned first = 16 - alignment16 (depth);
00247 unsigned idx;
00248 const float scale = depth_filter_.getFarClippingDistance () - depth_filter_.getNearClippingDistance ();
00249 const float offset = depth_filter_.getNearClippingDistance ();
00250 while (depth < depthEnd && idx++ < first)
00251 if (*depth != 0 && *depth != 1.0)
00252 *depth = *depth * scale + offset;
00253 else
00254 *depth = 0;
00255
00256
00257 unsigned last = (depth_filter_.getWidth () * depth_filter_.getHeight () - first) & 15;
00258 float* depth2 = depthEnd - last;
00259 while (depth2 < depthEnd)
00260 if (*depth2 != 0 && *depth != 1.0)
00261 *depth2 = *depth2 * scale + offset;
00262 else
00263 *depth2 = 0;
00264
00265 depthEnd -= last;
00266 }
00267
00268 const __m128* mmEnd = (__m128*) depthEnd;
00269 __m128* mmDepth = (__m128*) depth;
00270
00271 while (mmDepth < mmEnd)
00272 {
00273 *mmDepth = _mm_mul_ps (*mmDepth, mmScale);
00274 *mmDepth = _mm_add_ps (*mmDepth, mmNear);
00275 *mmDepth = _mm_and_ps (*mmDepth, _mm_and_ps (_mm_cmpneq_ps (*mmDepth, mmNear), _mm_cmpneq_ps (*mmDepth, mmFar)));
00276 ++mmDepth;
00277 }
00278 #else
00279 const float* depthEnd = depth + depth_filter_.getWidth () * depth_filter_.getHeight ();
00280 const float scale = depth_filter_.getFarClippingDistance () - depth_filter_.getNearClippingDistance ();
00281 const float offset = depth_filter_.getNearClippingDistance ();
00282 while (depth < depthEnd)
00283 {
00284
00285
00286 if (*depth != 0 && *depth != 1.0)
00287 *depth = *depth * scale + offset;
00288 else
00289 *depth = 0;
00290
00291 ++depth;
00292 }
00293 #endif
00294 }
00295
00296 void mesh_filter::MeshFilter::getFilteredLabels (unsigned* labels) const
00297 {
00298 depth_filter_.getColorBuffer ((unsigned char*) labels);
00299 }
00300
00301 void mesh_filter::MeshFilter::filter (const float* sensor_data, unsigned width, unsigned height, float fx, float fy, float cx, float cy) const
00302 {
00303 mesh_renderer_.setBufferSize (width, height);
00304 mesh_renderer_.setCameraParameters (fx, fy, cx, cy);
00305 mesh_renderer_.begin ();
00306 glEnable (GL_DEPTH_TEST);
00307 glEnable (GL_CULL_FACE);
00308 glCullFace (GL_FRONT);
00309 glDisable (GL_ALPHA_TEST);
00310
00311 GLuint padding_coefficients_id = glGetUniformLocation ( mesh_renderer_.getProgramID (), "padding_coefficients");
00312 glUniform3f (padding_coefficients_id, padding_coefficients_ [0], padding_coefficients_ [1], padding_coefficients_ [2]);
00313
00314 Affine3d transform;
00315 for (map<unsigned int, GLMesh*>::const_iterator meshIt = meshes_.begin (); meshIt != meshes_.end (); ++meshIt)
00316 if (transform_callback_ (meshIt->first, transform))
00317 meshIt->second->render (transform);
00318
00319 mesh_renderer_.end ();
00320
00321
00322 depth_filter_.setBufferSize (width, height);
00323 depth_filter_.setCameraParameters (fx, fy, cx, cy);
00324 depth_filter_.begin ();
00325 glEnable (GL_DEPTH_TEST);
00326 glEnable (GL_CULL_FACE);
00327 glCullFace (GL_BACK);
00328 glDisable (GL_ALPHA_TEST);
00329
00330 glUniform1f (near_location_, depth_filter_.getNearClippingDistance ());
00331 glUniform1f (far_location_, depth_filter_.getFarClippingDistance ());
00332 glUniform1f (shadow_threshold_location_, shadow_threshold_);
00333
00334 GLuint depth_texture = mesh_renderer_.getDepthTexture ();
00335 GLuint color_texture = mesh_renderer_.getColorTexture ();
00336
00337
00338 glActiveTexture (GL_TEXTURE0);
00339 glBindTexture ( GL_TEXTURE_2D, sensor_depth_texture_ );
00340
00341 float scale = 1.0 / (depth_filter_.getFarClippingDistance () - depth_filter_.getNearClippingDistance ());
00342 glPixelTransferf (GL_DEPTH_SCALE, scale);
00343 glPixelTransferf (GL_DEPTH_BIAS, -scale * depth_filter_.getNearClippingDistance ());
00344 glTexImage2D ( GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, width, height, 0, GL_DEPTH_COMPONENT, GL_FLOAT, sensor_data);
00345 glTexParameterf (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
00346 glTexParameterf (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
00347 glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
00348 glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
00349
00350
00351 glActiveTexture (GL_TEXTURE2);
00352 glBindTexture (GL_TEXTURE_2D, depth_texture);
00353
00354
00355 glActiveTexture (GL_TEXTURE4);
00356 glBindTexture (GL_TEXTURE_2D, color_texture);
00357
00358 glCallList (canvas_);
00359 glDisable ( GL_TEXTURE_2D );
00360 depth_filter_.end ();
00361 }
00362
00363 void mesh_filter::MeshFilter::setClippingRange (float near, float far)
00364 {
00365 mesh_renderer_.setClippingRange (near, far);
00366 depth_filter_.setClippingRange (near, far);
00367 }
00368
00369 void mesh_filter::MeshFilter::mergeVertices (const Mesh& mesh, Mesh& compressed)
00370 {
00371
00372
00373 static const double thresholdSQR = pow (0.00001, 2);
00374
00375 vector<unsigned> vertex_map (mesh.vertex_count);
00376 vector<Vector3d> vertices (mesh.vertex_count);
00377 vector<Vector3d> compressed_vertices;
00378 vector<Vector3i> triangles (mesh.triangle_count);
00379
00380 for (unsigned vIdx = 0; vIdx < mesh.vertex_count; ++vIdx)
00381 {
00382 vertices [vIdx][0] = mesh.vertices [3 * vIdx];
00383 vertices [vIdx][1] = mesh.vertices [3 * vIdx + 1];
00384 vertices [vIdx][2] = mesh.vertices [3 * vIdx + 2];
00385 vertex_map [vIdx] = vIdx;
00386 }
00387
00388 for (unsigned tIdx = 0; tIdx < mesh.triangle_count; ++tIdx)
00389 {
00390 triangles [tIdx][0] = mesh.triangles [3 * tIdx];
00391 triangles [tIdx][1] = mesh.triangles [3 * tIdx + 1];
00392 triangles [tIdx][2] = mesh.triangles [3 * tIdx + 2];
00393 }
00394
00395 for (unsigned vIdx1 = 0; vIdx1 < mesh.vertex_count; ++vIdx1)
00396 {
00397 if (vertex_map [vIdx1] != vIdx1)
00398 continue;
00399
00400 vertex_map [vIdx1] = compressed_vertices.size ();
00401 compressed_vertices.push_back (vertices[vIdx1]);
00402
00403 for (unsigned vIdx2 = vIdx1 + 1; vIdx2 < mesh.vertex_count; ++vIdx2)
00404 {
00405 double distanceSQR = (vertices[vIdx1] - vertices [vIdx2]).squaredNorm ();
00406 if (distanceSQR <= thresholdSQR)
00407 vertex_map [vIdx2] = vertex_map [vIdx1];
00408 }
00409 }
00410
00411
00412 for (unsigned tIdx = 0; tIdx < mesh.triangle_count; ++tIdx)
00413 {
00414 triangles [tIdx][0] = vertex_map [triangles [tIdx][0]];
00415 triangles [tIdx][1] = vertex_map [triangles [tIdx][1]];
00416 triangles [tIdx][2] = vertex_map [triangles [tIdx][2]];
00417 }
00418
00419 for (int vIdx = 0; vIdx < vertices.size (); ++vIdx)
00420 {
00421 if (vertices[vIdx][0] == vertices[vIdx][1] || vertices[vIdx][0] == vertices[vIdx][2] || vertices[vIdx][1] == vertices[vIdx][2])
00422 {
00423 vertices[vIdx] = vertices.back ();
00424 vertices.pop_back ();
00425 --vIdx;
00426 }
00427 }
00428
00429
00430 if (compressed.vertex_count > 0 && compressed.vertices)
00431 delete [] compressed.vertices;
00432 if (compressed.triangle_count > 0 && compressed.triangles)
00433 delete [] compressed.triangles;
00434 if (compressed.triangle_count > 0 && compressed.normals)
00435 delete [] compressed.normals;
00436
00437 compressed.vertex_count = compressed_vertices.size ();
00438 compressed.vertices = new double [compressed.vertex_count * 3];
00439 for (unsigned vIdx = 0; vIdx < compressed.vertex_count; ++vIdx)
00440 {
00441 compressed.vertices [3 * vIdx + 0] = compressed_vertices [vIdx][0];
00442 compressed.vertices [3 * vIdx + 1] = compressed_vertices [vIdx][1];
00443 compressed.vertices [3 * vIdx + 2] = compressed_vertices [vIdx][2];
00444 }
00445
00446 compressed.triangle_count = triangles.size ();
00447 compressed.triangles = new unsigned int [compressed.triangle_count * 3];
00448 for (unsigned tIdx = 0; tIdx < compressed.triangle_count; ++tIdx)
00449 {
00450 compressed.triangles [3 * tIdx + 0] = triangles [tIdx][0];
00451 compressed.triangles [3 * tIdx + 1] = triangles [tIdx][1];
00452 compressed.triangles [3 * tIdx + 2] = triangles [tIdx][2];
00453 }
00454
00455 compressed.normals = new double [compressed.triangle_count * 3];
00456 for (unsigned tIdx = 0; tIdx < compressed.triangle_count; ++tIdx)
00457 {
00458 Vector3d d1 = compressed_vertices [triangles[tIdx][1]] - compressed_vertices [triangles[tIdx][0]];
00459 Vector3d d2 = compressed_vertices [triangles[tIdx][2]] - compressed_vertices [triangles[tIdx][0]];
00460 Vector3d normal = d1.cross (d2);
00461 normal.normalize ();
00462 Vector3d normal_;
00463 normal_ [0] = mesh.normals [3 * tIdx];
00464 normal_ [1] = mesh.normals [3 * tIdx + 1];
00465 normal_ [2] = mesh.normals [3 * tIdx + 2];
00466 double cosAngle = normal.dot (normal_);
00467
00468 if (cosAngle < 0)
00469 {
00470 swap (compressed.triangles [3 * tIdx + 1], compressed.triangles [3 * tIdx + 2]);
00471 normal *= -1;
00472 }
00473
00474 compressed.normals [tIdx * 3 + 0] = normal [0];
00475 compressed.normals [tIdx * 3 + 1] = normal [1];
00476 compressed.normals [tIdx * 3 + 2] = normal [2];
00477 }
00478 }
00479
00480 string mesh_filter::MeshFilter::padding_vertex_shader_ =
00481 "#version 120\n"
00482 "uniform vec3 padding_coefficients;"
00483 "void main()"
00484 "{gl_FrontColor = gl_Color;"
00485 "vec4 vertex = gl_ModelViewMatrix * gl_Vertex;"
00486 "vec3 normal = normalize(gl_NormalMatrix * gl_Normal);"
00487 "float lambda = padding_coefficients.x * vertex.z * vertex.z + padding_coefficients.y * vertex.z + padding_coefficients.z;"
00488 "gl_Position = gl_ProjectionMatrix * (vertex + lambda * vec4(normal,0) );"
00489 "gl_Position.y = -gl_Position.y;}";
00490
00491 string mesh_filter::MeshFilter::filter_vertex_shader_ =
00492 "#version 120\n"
00493 "void main ()"
00494 "{"
00495 " gl_FrontColor = gl_Color;"
00496 " gl_TexCoord[0] = gl_MultiTexCoord0;"
00497 " gl_Position = gl_Vertex;"
00498 " gl_Position.w = 1.0;"
00499 "}";
00500
00501 string mesh_filter::MeshFilter::filter_fragment_shader_ =
00502 "#version 120\n"
00503 "uniform sampler2D sensor;"
00504 "uniform sampler2D depth;"
00505 "uniform sampler2D label;"
00506 "uniform float near;"
00507 "uniform float far;"
00508 "uniform float shadow_threshold;"
00509 "float f_n = far - near;"
00510 "float threshold = shadow_threshold / f_n;"
00511 "void main()"
00512 "{"
00513 " float dValue = float(texture2D(depth, gl_TexCoord[0].st));"
00514 " float zValue = dValue * near / (far - dValue * f_n);"
00515 " float diff = float(texture2D(sensor, gl_TexCoord[0].st)) - zValue;"
00516 " if (diff < 0) {"
00517 " gl_FragColor = vec4 (0, 0, 0, 0);"
00518 " gl_FragDepth = float(texture2D(sensor, gl_TexCoord[0].st));"
00519 " } else if (diff > threshold) {"
00520 " gl_FragColor = vec4 (0.003921569, 0, 0, 0);"
00521 " gl_FragDepth = float(texture2D(sensor, gl_TexCoord[0].st));"
00522 " } else {"
00523 " gl_FragColor = vec4 (1,1,1,1);"
00524 " gl_FragDepth = 0;"
00525 " }"
00526 "}";