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()
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));
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.
void read()
Read from BlackBoard into local copy.
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.