43 #include <Eigen/Eigen>
52 #ifdef HAVE_SSE_EXTENSIONS
53 #include <xmmintrin.h>
58 const std::string& render_vertex_shader,
59 const std::string& render_fragment_shader,
60 const std::string& filter_vertex_shader,
61 const std::string& filter_fragment_shader)
62 : sensor_parameters_(sensor_parameters.clone())
63 , next_handle_(FIRST_LABEL)
64 , min_handle_(FIRST_LABEL)
66 , transform_callback_(transform_callback)
68 , padding_offset_(0.01)
69 , shadow_threshold_(0.5)
72 std::thread([
this, render_vertex_shader, render_fragment_shader, filter_vertex_shader, filter_fragment_shader] {
73 run(render_vertex_shader, render_fragment_shader, filter_vertex_shader, filter_fragment_shader);
78 const std::string& render_fragment_shader,
79 const std::string& filter_vertex_shader,
80 const std::string& filter_fragment_shader)
82 mesh_renderer_ = std::make_shared<GLRenderer>(sensor_parameters_->getWidth(), sensor_parameters_->getHeight(),
83 sensor_parameters_->getNearClippingPlaneDistance(),
84 sensor_parameters_->getFarClippingPlaneDistance());
85 depth_filter_ = std::make_shared<GLRenderer>(sensor_parameters_->getWidth(), sensor_parameters_->getHeight(),
86 sensor_parameters_->getNearClippingPlaneDistance(),
87 sensor_parameters_->getFarClippingPlaneDistance());
89 mesh_renderer_->setShadersFromString(render_vertex_shader, render_fragment_shader);
90 depth_filter_->setShadersFromString(filter_vertex_shader, filter_fragment_shader);
92 depth_filter_->begin();
94 glGenTextures(1, &sensor_depth_texture_);
96 glUniform1i(glGetUniformLocation(depth_filter_->getProgramID(),
"sensor"), 0);
97 glUniform1i(glGetUniformLocation(depth_filter_->getProgramID(),
"depth"), 2);
98 glUniform1i(glGetUniformLocation(depth_filter_->getProgramID(),
"label"), 4);
100 shadow_threshold_location_ = glGetUniformLocation(depth_filter_->getProgramID(),
"shadow_threshold");
102 depth_filter_->end();
104 canvas_ = glGenLists(1);
105 glNewList(canvas_, GL_COMPILE);
110 glVertex3f(-1, -1, 1);
113 glVertex3f(1, -1, 1);
119 glVertex3f(-1, 1, 1);
128 std::unique_lock<std::mutex> lock(jobs_mutex_);
130 while (!jobs_queue_.empty())
132 jobs_queue_.front()->cancel();
136 jobs_condition_.notify_one();
137 filter_thread_.join();
143 std::unique_lock<std::mutex> _(jobs_mutex_);
144 jobs_queue_.push(job);
146 jobs_condition_.notify_one();
151 glDeleteLists(canvas_, 1);
152 glDeleteTextures(1, &sensor_depth_texture_);
155 mesh_renderer_.reset();
156 depth_filter_.reset();
161 mesh_renderer_->setBufferSize(width, height);
162 mesh_renderer_->setCameraParameters(width, width, width >> 1, height >> 1);
164 depth_filter_->setBufferSize(width, height);
165 depth_filter_->setCameraParameters(width, width, width >> 1, height >> 1);
170 std::unique_lock<std::mutex> _(transform_callback_mutex_);
171 transform_callback_ = transform_callback;
176 std::unique_lock<std::mutex> _(meshes_mutex_);
178 JobPtr job(
new FilterJob<void>([
this, &mesh] { addMeshHelper(next_handle_, mesh); }));
182 const std::size_t sz = min_handle_ + meshes_.size() + 1;
183 for (std::size_t i = min_handle_; i < sz; ++i)
184 if (meshes_.find(i) == meshes_.end())
189 min_handle_ = next_handle_;
195 meshes_[handle] = std::make_shared<GLMesh>(cmesh, handle);
200 std::unique_lock<std::mutex> _(meshes_mutex_);
207 throw std::runtime_error(
"Could not remove mesh. Mesh not found!");
208 min_handle_ = std::min(handle, min_handle_);
213 std::size_t erased = meshes_.erase(handle);
214 return (erased != 0);
219 shadow_threshold_ = threshold;
225 new FilterJob<void>([&renderer = *mesh_renderer_, labels] { renderer.getColorBuffer((
unsigned char*)labels); }));
232 JobPtr job1(
new FilterJob<void>([&renderer = *mesh_renderer_, depth] { renderer.getDepthBuffer(depth); }));
234 [¶meters = *sensor_parameters_, depth] { parameters.transformModelDepthToMetricDepth(depth); }));
236 std::unique_lock<std::mutex> lock(jobs_mutex_);
237 jobs_queue_.push(job1);
238 jobs_queue_.push(job2);
240 jobs_condition_.notify_one();
247 JobPtr job1(
new FilterJob<void>([&filter = *depth_filter_, depth] { filter.getDepthBuffer(depth); }));
249 [¶meters = *sensor_parameters_, depth] { parameters.transformFilteredDepthToMetricDepth(depth); }));
251 std::unique_lock<std::mutex> lock(jobs_mutex_);
252 jobs_queue_.push(job1);
253 jobs_queue_.push(job2);
255 jobs_condition_.notify_one();
262 JobPtr job(
new FilterJob<void>([&filter = *depth_filter_, labels] { filter.getColorBuffer((
unsigned char*)labels); }));
268 const std::string& render_fragment_shader,
269 const std::string& filter_vertex_shader,
270 const std::string& filter_fragment_shader)
272 initialize(render_vertex_shader, render_fragment_shader, filter_vertex_shader, filter_fragment_shader);
276 std::unique_lock<std::mutex> lock(jobs_mutex_);
278 if (jobs_queue_.empty())
279 jobs_condition_.wait(lock);
281 if (!jobs_queue_.empty())
283 JobPtr job = jobs_queue_.front();
295 if (type != GL_FLOAT && type != GL_UNSIGNED_SHORT)
297 std::stringstream msg;
298 msg <<
"unknown type \"" << type <<
"\". Allowed values are GL_FLOAT or GL_UNSIGNED_SHORT.";
299 throw std::runtime_error(msg.str());
302 JobPtr job(
new FilterJob<void>([
this, sensor_data, type] { doFilter(sensor_data, type); }));
310 std::unique_lock<std::mutex> _(transform_callback_mutex_);
312 mesh_renderer_->begin();
313 sensor_parameters_->setRenderParameters(*mesh_renderer_);
315 glEnable(GL_TEXTURE_2D);
316 glEnable(GL_DEPTH_TEST);
317 glDepthFunc(GL_LESS);
318 glEnable(GL_CULL_FACE);
319 glCullFace(GL_FRONT);
320 glDisable(GL_ALPHA_TEST);
323 GLuint padding_coefficients_id = glGetUniformLocation(mesh_renderer_->getProgramID(),
"padding_coefficients");
324 Eigen::Vector3f padding_coefficients =
325 sensor_parameters_->getPaddingCoefficients() * padding_scale_ + Eigen::Vector3f(0, 0, padding_offset_);
326 glUniform3f(padding_coefficients_id, padding_coefficients[0], padding_coefficients[1], padding_coefficients[2]);
329 for (
const std::pair<const MeshHandle, GLMeshPtr>& mesh : meshes_)
330 if (transform_callback_(mesh.first,
transform))
333 mesh_renderer_->end();
338 depth_filter_->begin();
339 sensor_parameters_->setFilterParameters(*depth_filter_);
340 glEnable(GL_TEXTURE_2D);
341 glEnable(GL_DEPTH_TEST);
342 glDepthFunc(GL_ALWAYS);
343 glDisable(GL_CULL_FACE);
344 glDisable(GL_ALPHA_TEST);
349 glUniform1f(shadow_threshold_location_, shadow_threshold_);
351 GLuint depth_texture = mesh_renderer_->getDepthTexture();
352 GLuint color_texture = mesh_renderer_->getColorTexture();
355 glActiveTexture(GL_TEXTURE0);
356 glBindTexture(GL_TEXTURE_2D, sensor_depth_texture_);
359 1.0 / (sensor_parameters_->getFarClippingPlaneDistance() - sensor_parameters_->getNearClippingPlaneDistance());
361 if (encoding == GL_UNSIGNED_SHORT)
367 glPixelTransferf(GL_DEPTH_SCALE, scale * 65.535);
369 glPixelTransferf(GL_DEPTH_SCALE, scale);
370 glPixelTransferf(GL_DEPTH_BIAS, -scale * sensor_parameters_->getNearClippingPlaneDistance());
372 glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, sensor_parameters_->getWidth(), sensor_parameters_->getHeight(), 0,
373 GL_DEPTH_COMPONENT, encoding, sensor_data);
374 glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
375 glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
376 glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
377 glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
380 glActiveTexture(GL_TEXTURE2);
381 glBindTexture(GL_TEXTURE_2D, depth_texture);
384 glActiveTexture(GL_TEXTURE4);
385 glBindTexture(GL_TEXTURE_2D, color_texture);
387 depth_filter_->end();
392 padding_offset_ = offset;
397 padding_scale_ = scale;