Taken into account image scaling to gui for point correspondence. Update...

Taken into account image scaling to gui for point correspondence. Update filter when loaded parameters. Launch camera name as argument.

include/iri_color_calibration_alg_node.h

@@ -120,6 +120,7 @@ class ColorCalibrationAlgNode : public algorithm_base::IriBaseAlgorithm<ColorCal
     bool get_hsv_pixel(unsigned int row, unsigned int col, unsigned int &hue, unsigned int &sat, unsigned int &val);
     bool get_rgb_pixel(unsigned int row, unsigned int col, unsigned char &R, unsigned char &G, unsigned char &B);
 
+    void update_filter();
     void update_size_filter(unsigned int max, unsigned int min);
     void get_size_filter(unsigned int &max, unsigned int &min);
     void add_pixel_filter(unsigned int &hue, unsigned int &sat, unsigned int &val);

include/main_gui.h

@@ -31,6 +31,9 @@ class MainGui : public QWidget
     pthread_t thread;
     static void *rosThread(void *param);
     void show_filter_params(void);
+    
+    int img_width;
+    int img_height;
 
   public:
     Ui::color_calibration *ui;

launch/color_calibration.launch

@@ -3,12 +3,14 @@
  
   <include file="$(find iri_blob_detector)/launch/blob_detector.launch" />
  
+  <arg name="camera" default="usb_cam"/> <!-- camera/rgb, usb_cam -->
+ 
   <node pkg ="iri_color_calibration"
         type="iri_color_calibration"
         name="color_calibration"
         output="screen">
-      <remap from="~/original_image/image_raw" to="/usb_cam/image_raw"/>
-      <remap from="~/original_image/camera_info" to="/usb_cam/camera_info"/>
+      <remap from="~/original_image/image_raw" to="/$(arg camera)/image_raw"/>
+      <remap from="~/original_image/camera_info" to="/$(arg camera)/camera_info"/>
       <remap from="~/segmented_image/image_raw" to="/blob/image_out/image_raw"/>
       <remap from="~/segmented_image/camera_info" to="/blob/image_out/camera_info"/>
       <remap from="~/set_filter" to="/blob/set_filter"/>

launch/color_calibration_sim.launch

@@ -2,13 +2,15 @@
 <launch>
  
   <include file="$(find iri_blob_detector)/launch/blob_detector_sim.launch" />
+  
+  <arg name="camera" default="usb_cam"/> <!-- camera/rgb, usb_cam -->
  
   <node pkg ="iri_color_calibration"
         type="iri_color_calibration"
         name="color_calibration"
         output="screen">
-      <remap from="~/original_image/image_raw" to="/usb_cam/image_raw"/>
-      <remap from="~/original_image/camera_info" to="/usb_cam/camera_info"/>
+      <remap from="~/original_image/image_raw" to="/$(arg camera)/image_raw"/>
+      <remap from="~/original_image/camera_info" to="/$(arg camera)/camera_info"/>
       <remap from="~/segmented_image/image_raw" to="/blob/image_out/image_raw"/>
       <remap from="~/segmented_image/camera_info" to="/blob/image_out/camera_info"/>
       <remap from="~/set_filter" to="/blob/set_filter"/>

src/iri_color_calibration_alg_node.cpp

@@ -282,6 +282,19 @@ void ColorCalibrationAlgNode::filter_mutex_exit(void)
   pthread_mutex_unlock(&this->filter_mutex_);
 }
 
+void ColorCalibrationAlgNode::update_filter()
+{
+  this->filter_mutex_enter();
+  if(this->set_filter_client_.call(this->set_filter_srv_))
+  {
+    if(this->set_filter_srv_.response.success==false)
+      ROS_INFO("ColorCalibrationAlgNode:: Impossible to set the HSV filter");
+  }
+  else
+    ROS_INFO("ColorCalibrationAlgNode:: Failed to set the HSV filter");
+  this->filter_mutex_exit();
+}
+
 void ColorCalibrationAlgNode::update_size_filter(unsigned int max, unsigned int min)
 {
   this->filter_mutex_enter();

src/main_gui.cpp

@@ -28,6 +28,10 @@ MainGui::MainGui(int argc, char** argv, QWidget *parent) :
   // initialize image selection
   this->ui->show_original_img->setChecked(true);
   this->ui->show_segmented_img->setChecked(false);
+  
+  this->ui->image->setAlignment(Qt::AlignHCenter | Qt::AlignVCenter);
+  this->ui->blob_min_size->setText("10");
+  this->ui->blob_max_size->setText("100000");
 
   // initialize pixel list
   this->ui->pixel_list->horizontalHeader()->setResizeMode(QHeaderView::Stretch);
@@ -59,13 +63,11 @@ void MainGui::timerDone()
   std::string encoding;
   bool format_ok=true,new_img=false;
   uchar* data;
-  int width;
-  int height;
 
   if(this->ui->show_original_img->isChecked())
-    new_img=this->node->get_original_image(data,width, height, encoding);
+    new_img=this->node->get_original_image(data,this->img_width, this->img_height, encoding);
   else if(this->ui->show_segmented_img->isChecked())
-    new_img=this->node->get_segmented_image(data,width, height, encoding);
+    new_img=this->node->get_segmented_image(data,this->img_width, this->img_height, encoding);
   if(new_img)
   {
     if(encoding=="rgb8")
@@ -78,7 +80,7 @@ void MainGui::timerDone()
       format_ok=false;
     if(format_ok)
     {
-      QImage myImage(data, width, height, format);
+      QImage myImage(data, this->img_width, this->img_height, format);
       if(encoding=="mono8")
         myImage.setColorTable(this->grayscale_color_table);
       else
@@ -97,6 +99,7 @@ void MainGui::timerDone()
           painter.drawEllipse(QPoint(blobs[i].center_x,blobs[i].center_y),(int)blobs[i].size,(int)blobs[i].size);     
         painter.end();
       }
+      //ROS_INFO("ui w,h=%d,%d", this->ui->image->width(), this->ui->image->height());
       QPixmap p(QPixmap::fromImage(myImage));
       QPixmap p2= p.scaled(this->ui->image->width(),this->ui->image->height(),Qt::KeepAspectRatio);
       this->ui->image->setPixmap(p2);
@@ -119,6 +122,25 @@ void MainGui::on_image_mousePressed(QPoint &point)
   unsigned int H,S,V;
   QString item_text;
 
+  double scale_x = this->img_width*1.0/this->ui->image->width();
+  double scale_y = this->img_height*1.0/this->ui->image->height();
+  double scale = std::max(scale_x, scale_y);
+
+  double dx = (this->ui->image->width()  - this->img_width/scale) /2.0;
+  double dy = (this->ui->image->height() - this->img_height/scale)/2.0;
+  
+  point.setX(point.x()-dx);
+  point.setY(point.y()-dy);
+  
+  point.setX(point.x()*scale);
+  point.setY(point.y()*scale);
+  
+  if(point.x()<0 || point.y()<0 || point.x() > this->img_width || point.y() > this->img_height)
+  {
+    ROS_WARN("iri_color_calibration::main_gui: clicked point out of image. Ignored");
+    return;
+  }
+
   // get the color of the pixel
   this->node->get_rgb_pixel(point.y(),point.x(),R,G,B);
   this->node->get_hsv_pixel(point.y(),point.x(),H,S,V);
@@ -130,6 +152,7 @@ void MainGui::on_image_mousePressed(QPoint &point)
   this->ui->pixel_list->setItem(this->ui->pixel_list->rowCount()-1,1,new QTableWidgetItem(item_text));
   item_text.setNum(B);
   this->ui->pixel_list->setItem(this->ui->pixel_list->rowCount()-1,2,new QTableWidgetItem(item_text));
+  this->ui->pixel_list->scrollToBottom();
   // update the filter coefficients
   this->node->add_pixel_filter(H,S,V);
   this->show_filter_params();
@@ -183,6 +206,7 @@ void MainGui::on_load_params_pressed(void)
   this->ui->pixel_list->clearContents();
   this->ui->pixel_list->setRowCount(0);
   this->show_filter_params();
+  this->node->update_filter();
 }
 
 void MainGui::on_save_params_pressed(void)