25 #include <blackboard/remote.h> 26 #include <interfaces/OpenCVStereoParamsInterface.h> 27 #include <netcomm/fawkes/client.h> 28 #include <fvcams/net.h> 29 #include <fvutils/color/conversions.h> 31 #include <gui_utils/service_chooser_dialog.h> 32 #include <gui_utils/interface_dispatcher.h> 43 #define FIREVISION_PORT 2208 44 #define BB2_IMG_RECT_LEFT "bumblebee2-rgb-rectified-left" 45 #define BB2_IMG_DISPARITY "bumblebee2-disparity" 46 #define IMG_UPDATE_INTERVAL 200 58 const Glib::RefPtr<Gtk::Builder> &builder)
59 : Gtk::Window(cobject)
63 cam_left_rectified_ = cam_disparity_ = NULL;
64 buffer_rgb_disparity_ = buffer_rgb_rect_left_ = NULL;
66 builder->get_widget(
"cmb_pre_filter_type", cmb_pre_filter_type);
67 builder->get_widget(
"lab_pre_filter_type", lab_pre_filter_type);
68 builder->get_widget(
"lab_pre_filter_size", lab_pre_filter_size);
69 builder->get_widget(
"lab_pre_filter_cap", lab_pre_filter_cap);
70 builder->get_widget(
"lab_sad_window_size", lab_sad_window_size);
71 builder->get_widget(
"lab_min_disparity", lab_min_disparity);
72 builder->get_widget(
"lab_num_disparities", lab_num_disparities);
73 builder->get_widget(
"lab_texture_threshold", lab_texture_threshold);
74 builder->get_widget(
"lab_uniqueness_ratio", lab_uniqueness_ratio);
75 builder->get_widget(
"lab_speckle_window_size", lab_speckle_window_size);
76 builder->get_widget(
"lab_speckle_range", lab_speckle_range);
77 builder->get_widget(
"lab_try_smaller_windows", lab_try_smaller_windows);
78 builder->get_widget(
"hsc_pre_filter_size", hsc_pre_filter_size);
79 builder->get_widget(
"hsc_pre_filter_cap", hsc_pre_filter_cap);
80 builder->get_widget(
"hsc_sad_window_size", hsc_sad_window_size);
81 builder->get_widget(
"hsc_min_disparity", hsc_min_disparity);
82 builder->get_widget(
"hsc_num_disparities", hsc_num_disparities);
83 builder->get_widget(
"hsc_texture_threshold", hsc_texture_threshold);
84 builder->get_widget(
"hsc_uniqueness_ratio", hsc_uniqueness_ratio);
85 builder->get_widget(
"hsc_speckle_window_size", hsc_speckle_window_size);
86 builder->get_widget(
"cb_try_smaller_windows", cb_try_smaller_windows);
87 builder->get_widget(
"hsc_speckle_range", hsc_speckle_range);
88 builder->get_widget(
"tb_connection", tb_connection);
89 builder->get_widget(
"tb_exit", tb_exit);
90 builder->get_widget(
"img_left_rectified", img_left_rectified);
91 builder->get_widget(
"img_disparity", img_disparity);
92 builder->get_widget(
"img_writer", img_writer);
94 cmb_pre_filter_type->signal_changed().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_pre_filter_type_changed));
95 hsc_pre_filter_size->signal_value_changed().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_pre_filter_size_changed));
96 hsc_pre_filter_cap->signal_value_changed().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_pre_filter_cap_changed));
97 hsc_sad_window_size->signal_value_changed().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_sad_window_size_changed));
98 hsc_min_disparity->signal_value_changed().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_min_disparity_changed));
99 hsc_num_disparities->signal_value_changed().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_num_disparities_changed));
100 hsc_texture_threshold->signal_value_changed().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_texture_threshold_changed));
101 hsc_uniqueness_ratio->signal_value_changed().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_uniqueness_ratio_changed));
102 hsc_speckle_window_size->signal_value_changed().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_speckle_window_size_changed));
103 hsc_speckle_range->signal_value_changed().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_speckle_range_changed));
104 cb_try_smaller_windows->signal_toggled().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_try_smaller_windows_toggled));
106 tb_connection->signal_clicked().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_connection_clicked));
107 tb_exit->signal_clicked().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_exit_clicked));
109 connection_dispatcher.
signal_connected().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_connect));
110 connection_dispatcher.
signal_disconnected().connect(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::on_disconnect));
126 Bumblebee2CalibGtkWindow::init()
132 Bumblebee2CalibGtkWindow::on_pre_filter_type_changed()
135 if (cmb_pre_filter_type->get_active_row_number() == 1) {
136 new_type = OpenCVStereoParamsInterface::PFT_NORMALIZED_RESPONSE;
138 new_type = OpenCVStereoParamsInterface::PFT_XSOBEL;
151 Bumblebee2CalibGtkWindow::on_pre_filter_size_changed()
153 unsigned new_value = (
unsigned int)hsc_pre_filter_size->get_value() * 2 + 1;
156 printf(
"Setting pre filter size %u\n", new_value);
165 Bumblebee2CalibGtkWindow::on_pre_filter_cap_changed()
167 unsigned int new_value = (
unsigned int)hsc_pre_filter_cap->get_value();
169 printf(
"Setting pre filter cap %u\n", new_value);
178 Bumblebee2CalibGtkWindow::on_sad_window_size_changed()
180 unsigned int new_value = (
unsigned int)hsc_sad_window_size->get_value() * 2 + 1;
183 printf(
"Setting SAD window size %u\n", new_value);
192 Bumblebee2CalibGtkWindow::on_min_disparity_changed()
194 int new_value = (int)hsc_min_disparity->get_value();
196 printf(
"Setting min disparity %i\n", new_value);
205 Bumblebee2CalibGtkWindow::on_num_disparities_changed()
207 unsigned int new_value = (
unsigned int)hsc_num_disparities->get_value() * 16;
209 printf(
"Setting num disparities %u\n", new_value);
218 Bumblebee2CalibGtkWindow::on_texture_threshold_changed()
220 unsigned int new_value = (
unsigned int)hsc_texture_threshold->get_value();
224 printf(
"Setting texture threshold %u\n", new_value);
233 Bumblebee2CalibGtkWindow::on_uniqueness_ratio_changed()
235 unsigned int new_value = (
unsigned int)hsc_uniqueness_ratio->get_value();
239 printf(
"Setting uniqueness ratio %u\n", new_value);
248 Bumblebee2CalibGtkWindow::on_speckle_window_size_changed()
250 unsigned int new_value = (
unsigned int)hsc_speckle_window_size->get_value();
253 printf(
"Setting speckle window size %u\n", new_value);
262 Bumblebee2CalibGtkWindow::on_speckle_range_changed()
264 unsigned int new_value = (
unsigned int)hsc_speckle_range->get_value();
266 printf(
"Setting speckle range %u\n", new_value);
276 Bumblebee2CalibGtkWindow::on_connection_clicked()
289 Bumblebee2CalibGtkWindow::on_try_smaller_windows_toggled()
291 bool new_value = cb_try_smaller_windows->get_active();
296 printf(
"%sabling smaller windows\n", new_value ?
"En" :
"Dis");
305 Bumblebee2CalibGtkWindow::on_connect()
312 if (! params_if_->has_writer()) {
313 throw Exception(
"No writer for parameter blackboard interface");
317 ifd_params_->
signal_data_changed().connect(sigc::hide(sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::update_param_values)));
321 tb_connection->set_stock_id(Gtk::Stock::DISCONNECT);
323 update_param_values();
325 switch (params_if_->pre_filter_type()) {
326 case OpenCVStereoParamsInterface::PFT_XSOBEL:
327 cmb_pre_filter_type->set_active(0);
330 cmb_pre_filter_type->set_active(1);
333 hsc_pre_filter_size->set_value(params_if_->pre_filter_size() / 2);
334 hsc_pre_filter_cap->set_value(params_if_->pre_filter_cap());
335 hsc_sad_window_size->set_value(params_if_->sad_window_size() / 2);
336 hsc_min_disparity->set_value(params_if_->min_disparity());
337 hsc_num_disparities->set_value(params_if_->num_disparities() / 16);
338 hsc_texture_threshold->set_value(params_if_->texture_threshold());
339 hsc_uniqueness_ratio->set_value(params_if_->uniqueness_ratio());
340 hsc_speckle_window_size->set_value(params_if_->speckle_window_size());
341 hsc_speckle_range->set_value(params_if_->speckle_range());
342 cb_try_smaller_windows->set_active(params_if_->is_try_smaller_windows());
344 cmb_pre_filter_type->set_sensitive(
true);
345 hsc_pre_filter_size->set_sensitive(
true);
346 hsc_pre_filter_cap->set_sensitive(
true);
347 hsc_sad_window_size->set_sensitive(
true);
348 hsc_min_disparity->set_sensitive(
true);
349 hsc_num_disparities->set_sensitive(
true);
350 hsc_texture_threshold->set_sensitive(
true);
351 hsc_uniqueness_ratio->set_sensitive(
true);
352 hsc_speckle_window_size->set_sensitive(
true);
353 hsc_speckle_range->set_sensitive(
true);
354 cb_try_smaller_windows->set_sensitive(
true);
357 FIREVISION_PORT, BB2_IMG_RECT_LEFT,
false);
358 cam_left_rectified_->
open();
359 cam_left_rectified_->
start();
360 printf(
"Colorspace: %s\n", colorspace_to_string(cam_left_rectified_->
colorspace()));
363 FIREVISION_PORT, BB2_IMG_DISPARITY,
false);
364 cam_disparity_->
open();
365 cam_disparity_->
start();
366 buffer_rgb_disparity_ =
368 buffer_rgb_rect_left_ =
369 malloc_buffer(RGB, cam_left_rectified_->
pixel_width(),
372 sconn_update_images_ =
373 Glib::signal_timeout().connect(
374 sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::update_images), IMG_UPDATE_INTERVAL);
377 this->set_title(std::string(
"Bumblebee2 Calibration @ ") +
381 Glib::ustring message = *(e.
begin());
382 Gtk::MessageDialog md(*
this, message,
false,
383 Gtk::MESSAGE_ERROR, Gtk::BUTTONS_OK,
385 md.set_title(
"Connection failed");
389 bb_->
close(params_if_);
396 delete cam_left_rectified_;
397 delete cam_disparity_;
398 if (buffer_rgb_disparity_) free(buffer_rgb_disparity_);
399 if (buffer_rgb_rect_left_) free(buffer_rgb_rect_left_);
400 cam_left_rectified_ = cam_disparity_ = NULL;
401 buffer_rgb_disparity_ = buffer_rgb_rect_left_ = NULL;
409 Bumblebee2CalibGtkWindow::on_disconnect()
411 sconn_update_images_.disconnect();
413 img_writer->set_from_icon_name(Gtk::Stock::NO.
id,
414 Gtk::IconSize(Gtk::ICON_SIZE_SMALL_TOOLBAR));
415 img_writer->set_tooltip_text(
"Not connected and thus no writer");
417 img_disparity->clear();
418 img_disparity->set(
"gtk-missing-image");
420 img_left_rectified->clear();
421 img_left_rectified->set(
"gtk-missing-image");
423 lab_pre_filter_size->set_text(
"");
424 lab_pre_filter_cap->set_text(
"");
425 lab_sad_window_size->set_text(
"");
426 lab_min_disparity->set_text(
"");
427 lab_num_disparities->set_text(
"");
428 lab_texture_threshold->set_text(
"");
429 lab_uniqueness_ratio->set_text(
"");
430 lab_speckle_window_size->set_text(
"");
431 lab_speckle_range->set_text(
"");
432 lab_try_smaller_windows->set_text(
"");
434 cmb_pre_filter_type->set_sensitive(
false);
435 hsc_pre_filter_size->set_sensitive(
false);
436 hsc_pre_filter_cap->set_sensitive(
false);
437 hsc_sad_window_size->set_sensitive(
false);
438 hsc_min_disparity->set_sensitive(
false);
439 hsc_num_disparities->set_sensitive(
false);
440 hsc_texture_threshold->set_sensitive(
false);
441 hsc_uniqueness_ratio->set_sensitive(
false);
442 hsc_speckle_window_size->set_sensitive(
false);
443 hsc_speckle_range->set_sensitive(
false);
444 cb_try_smaller_windows->set_sensitive(
false);
448 bb_->
close(params_if_);
456 if (cam_disparity_) {
457 cam_disparity_->
stop();
458 cam_disparity_->
close();
459 delete cam_disparity_;
460 cam_disparity_ = NULL;
462 if (buffer_rgb_disparity_) free(buffer_rgb_disparity_);
463 buffer_rgb_disparity_ = NULL;
465 if (buffer_rgb_rect_left_) free(buffer_rgb_rect_left_);
466 buffer_rgb_rect_left_ = NULL;
468 if (cam_left_rectified_) {
469 cam_left_rectified_->
stop();
470 cam_left_rectified_->
close();
471 delete cam_left_rectified_;
472 cam_left_rectified_ = NULL;
475 tb_connection->set_stock_id(Gtk::Stock::CONNECT);
478 this->set_title(
"Bumblebee2 Calibration");
483 Bumblebee2CalibGtkWindow::update_param_values()
487 switch (params_if_->pre_filter_type()) {
488 case OpenCVStereoParamsInterface::PFT_XSOBEL:
489 lab_pre_filter_type->set_text(
"XSOBEL");
492 lab_pre_filter_type->set_text(
"NORM RESP");
495 lab_pre_filter_size->set_text(convert_float2str(params_if_->pre_filter_size(), 0));
496 lab_pre_filter_cap->set_text(convert_float2str(params_if_->pre_filter_cap(), 0));
497 lab_sad_window_size->set_text(convert_float2str(params_if_->sad_window_size(), 0));
498 lab_min_disparity->set_text(convert_float2str(params_if_->min_disparity(), 0));
499 lab_num_disparities->set_text(convert_float2str(params_if_->num_disparities(), 0));
500 lab_texture_threshold->set_text(convert_float2str(params_if_->texture_threshold(), 0));
501 lab_uniqueness_ratio->set_text(convert_float2str(params_if_->uniqueness_ratio(), 0));
502 lab_speckle_window_size->set_text(convert_float2str(params_if_->speckle_window_size(), 0));
503 lab_speckle_range->set_text(convert_float2str(params_if_->speckle_range(), 0));
504 lab_try_smaller_windows->set_text(params_if_->is_try_smaller_windows() ?
"true" :
"false");
509 Bumblebee2CalibGtkWindow::dont_destroy(
const guint8 *data)
514 Bumblebee2CalibGtkWindow::update_images()
516 if (bb_ && params_if_ && params_if_->has_writer())
518 if (img_writer->get_icon_name() != Gtk::Stock::YES.id) {
519 img_writer->set_from_icon_name(Gtk::Stock::YES.
id,
520 Gtk::IconSize(Gtk::ICON_SIZE_SMALL_TOOLBAR));
521 img_writer->set_tooltip_text(
"Writer for blackboard interface exists");
524 cam_left_rectified_->
capture();
525 unsigned int rlwidth = cam_left_rectified_->
pixel_width();
526 unsigned int rlheight = cam_left_rectified_->
pixel_height();
527 convert(cam_left_rectified_->
colorspace(), RGB,
528 cam_left_rectified_->
buffer(), buffer_rgb_rect_left_,
532 Glib::RefPtr<Gdk::Pixbuf> image =
533 Gdk::Pixbuf::create_from_data(buffer_rgb_rect_left_, Gdk::COLORSPACE_RGB,
535 rlwidth, rlheight, 3 * rlwidth,
536 sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::dont_destroy));
538 image = image->scale_simple(320, 240, Gdk::INTERP_BILINEAR);
540 img_left_rectified->set(image);
546 cam_disparity_->
buffer(), buffer_rgb_disparity_,
550 Glib::RefPtr<Gdk::Pixbuf> dimage =
551 Gdk::Pixbuf::create_from_data(buffer_rgb_disparity_, Gdk::COLORSPACE_RGB,
553 rlwidth, rlheight, 3 * rlwidth,
554 sigc::mem_fun(*
this, &Bumblebee2CalibGtkWindow::dont_destroy));
555 img_disparity->set(dimage);
557 if (img_writer->get_icon_name() != Gtk::Stock::NO.id) {
558 img_writer->set_from_icon_name(Gtk::Stock::NO.
id,
559 Gtk::IconSize(Gtk::ICON_SIZE_SMALL_TOOLBAR));
560 img_writer->set_tooltip_text(
"There is no blackboard writer for the interface");
568 Bumblebee2CalibGtkWindow::on_exit_clicked()
580 Bumblebee2CalibGtkWindow::convert_float2str(
float f,
unsigned int width)
582 #if GLIBMM_MAJOR_VERSION > 2 || ( GLIBMM_MAJOR_VERSION == 2 && GLIBMM_MINOR_VERSION >= 16 ) 583 return Glib::ustring::format(std::fixed, std::setprecision(width), f);
585 std::ostringstream ss;
586 ss << std::fixed << std::setprecision(width);
589 return Glib::locale_to_utf8(ss.str());
596 Bumblebee2CalibGtkWindow::convert_str2float(Glib::ustring sn,
float *f)
599 *f = strtof(sn.c_str(), &endptr);
600 if ( endptr[0] != 0 ) {
601 Glib::ustring s(
"Could not convert string to valid number: ");
602 s.append(sn, 0, sn.length() - strlen(endptr));
603 s +=
" >>><b>";
605 s +=
"</b><<< ";
606 s.append(endptr + 1, strlen(endptr) - 1);
608 Gtk::MessageDialog md(*
this, s,
611 md.set_title(
"Invalid value");
virtual unsigned int pixel_height()
Height of image in pixels.
sigc::signal< void > signal_disconnected()
Get "disconnected" signal.
SetSADWindowSizeMessage Fawkes BlackBoard Interface Message.
sigc::signal< void > signal_connected()
Get "connected" signal.
virtual void capture()
Capture an image.
virtual colorspace_t colorspace()
Colorspace of returned image.
uint32_t uniqueness_ratio() const
Get uniqueness_ratio value.
Fawkes library namespace.
uint32_t speckle_window_size() const
Get speckle_window_size value.
virtual unsigned char * buffer()
Get access to current image buffer.
void disconnect()
Disconnect socket.
PreFilterType pre_filter_type() const
Get pre_filter_type value.
SetPreFilterTypeMessage Fawkes BlackBoard Interface Message.
SetPreFilterSizeMessage Fawkes BlackBoard Interface Message.
virtual void open()
Open the camera.
int32_t min_disparity() const
Get min_disparity value.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
uint32_t pre_filter_cap() const
Get pre_filter_cap value.
uint32_t texture_threshold() const
Get texture_threshold value.
SetTextureThresholdMessage Fawkes BlackBoard Interface Message.
SetTrySmallerWindowsMessage Fawkes BlackBoard Interface Message.
SetSpeckleRangeMessage Fawkes BlackBoard Interface Message.
uint32_t num_disparities() const
Get num_disparities value.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
~Bumblebee2CalibGtkWindow()
Destructor.
uint32_t speckle_range() const
Get speckle_range value.
SetSpeckleWindowSizeMessage Fawkes BlackBoard Interface Message.
Base class for exceptions in Fawkes.
OpenCVStereoParamsInterface Fawkes BlackBoard Interface.
virtual void start()
Start image transfer from the camera.
void run_and_connect()
Run dialog and try to connect.
const char * tostring_PreFilterType(PreFilterType value) const
Convert PreFilterType constant to string.
SetPreFilterCapMessage Fawkes BlackBoard Interface Message.
bool has_writer() const
Check if there is a writer for the interface.
virtual void dispose_buffer()
Dispose current buffer.
virtual unsigned int pixel_width()
Width of image in pixels.
iterator begin()
Get iterator for messages.
SetUniquenessRatioMessage Fawkes BlackBoard Interface Message.
Bumblebee2CalibGtkWindow(BaseObjectType *cobject, const Glib::RefPtr< Gtk::Builder > &builder)
Constructor.
virtual void close()
Close camera.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
bool is_try_smaller_windows() const
Get try_smaller_windows value.
uint32_t pre_filter_size() const
Get pre_filter_size value.
PreFilterType
The type used for the pre-filter to come up with the features for the correspondence matching...
virtual void stop()
Stop image transfer from the camera.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
const char * get_hostname() const
Get the client's hostname.
SetNumDisparitiesMessage Fawkes BlackBoard Interface Message.
SetMinDisparityMessage Fawkes BlackBoard Interface Message.
bool connected() const
Check if connection is alive.
FawkesNetworkClient * get_client()
Get client.
uint32_t sad_window_size() const
Get sad_window_size value.
Interface listener with dispatcher.
sigc::signal< void, Interface * > signal_data_changed()
Get "data changed" signal.
virtual void close(Interface *interface)=0
Close interface.