object_segmentation_gui: object_segmentation_gui: object_segmentation_rviz

00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "object_segmentation_gui/object_segmentation_rviz_ui.h"
00031 
00032 #include "rviz_interaction_tools/image_tools.h"
00033 #include "rviz_interaction_tools/image_overlay.h"
00034 #include "rviz_interaction_tools/camera_tools.h"
00035 #include "rviz_interaction_tools/unique_string_manager.h"
00036 
00037 #include <rviz/window_manager_interface.h>
00038 #include <rviz/render_panel.h>
00039 #include <rviz/visualization_manager.h>
00040 #include <rviz/validate_floats.h>
00041 #include <rviz/frame_manager.h>
00042 
00043 #include <OGRE/OgreRenderWindow.h>
00044 #include <OGRE/OgreRoot.h>
00045 
00046 #include "tabletop_object_detector/marker_generator.h"
00047 
00048 #include "pcl/point_cloud.h"
00049 #include "pcl/point_types.h"
00050 #include "pcl/filters/statistical_outlier_removal.h"
00051 #include "pcl/filters/voxel_grid.h"
00052 
00053 #include <sensor_msgs/PointCloud.h>
00054 #include <sensor_msgs/PointCloud2.h>
00055 #include <sensor_msgs/point_cloud_conversion.h>
00056 #include <sensor_msgs/image_encodings.h>
00057 
00058 namespace enc = sensor_msgs::image_encodings;
00059 
00060 // Does not work right now, probably colliding with OGRE in Rviz
00061 //#define USE_CUDA
00062 
00063 #define MAX_FG 6
00064 
00065 namespace object_segmentation_gui
00066 {
00067 
00068 typedef pcl::PointXYZ PCL_Point;
00069 
00070 static const char
00071     * HELP_TEXT =
00072         "Left Mouse Button: Click on objects or drag a rectangle to segment them. \n\
00073      Segment Button: Start Segmentation. \n\
00074      Reset Button: Reset Segmentation and Seeds. \n\
00075      Cancel Button: Cancel Segmentation. \n\
00076      OK Button: Accept Segmentation. ";
00077 
00078 ObjectSegmentationRvizUI::ObjectSegmentationRvizUI(
00079     rviz::VisualizationManager *visualization_manager ) :
00080   ObjectSegmentationFrame(
00081       visualization_manager->getWindowManager()->getParentWindow()),
00082       object_segmentation_server_(0), nh_(""), priv_nh_("~"), n_iter_(2),
00083       use_gpu_(false), object_segmenter_(0), num_fg_hypos_(0), running_(false),
00084       paused_(true), num_markers_published_(1), current_marker_id_(1)
00085 {
00086 
00088   //    image_transport::ImageTransport it_(nh_);
00089   //    image_pub_ = it_.advertise("rgb_out", 1);
00090 
00091   rviz_interaction_tools::UniqueStringManager usm;
00092 
00093   //construct basic ogre scene
00094   scene_manager_ = Ogre::Root::getSingleton().createSceneManager(
00095       Ogre::ST_GENERIC, usm.unique("ObjectSegmentationRvizUI"));
00096 
00097   scene_root_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00098 
00099   image_overlay_ = new rviz_interaction_tools::ImageOverlay(scene_root_,
00100       Ogre::RENDER_QUEUE_OVERLAY - 2);
00101 
00102   //put a render panel into the window
00103   createRenderPanel(visualization_manager);
00104 
00105   SetTitle(wxString::FromAscii("Interactive Object Segmentation"));
00106   bottom_label_->SetLabel(wxString::FromAscii(HELP_TEXT));
00107 
00108   click_info_.down_ = false;
00109   click_info_.down_x_ = click_info_.down_y_ = 0;
00110 
00111   marker_pub_ = nh_.advertise<visualization_msgs::Marker> (nh_.resolveName(
00112       "tabletop_segmentation_markers"), 10);
00113 
00114   box_object_ = scene_manager_->createManualObject("SelectRegionIndicator");
00115   box_object_->setUseIdentityProjection(true);
00116   box_object_->setUseIdentityView(true);
00117   box_object_->setDynamic(true);
00118   box_object_->begin("BaseWhiteNoLighting",
00119       Ogre::RenderOperation::OT_LINE_STRIP);
00120   box_object_->position(0.0, 0.0, 0.0);
00121   box_object_->position(0.0, 0.0, 0.0);
00122   box_object_->position(0.0, 0.0, 0.0);
00123   box_object_->position(0.0, 0.0, 0.0);
00124   box_object_->index(0);
00125   box_object_->index(1);
00126   box_object_->index(2);
00127   box_object_->index(3);
00128   box_object_->index(0);
00129   box_object_->end();
00130   Ogre::AxisAlignedBox aabInf;
00131   aabInf.setInfinite();
00132   box_object_->setBoundingBox(aabInf);
00133   //click indicator will be rendered last
00134   box_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY);
00135   scene_root_->attachObject(box_object_);
00136 
00137   priv_nh_.param<int> ("inlier_threshold", inlier_threshold_, 300);
00138   priv_nh_.param<double> ("up_direction", up_direction_, -1.0);
00139 
00140   priv_nh_.param<double> ("mean_k", mean_k_, 50.0);
00141   priv_nh_.param<double> ("std", std_, 1.0);
00142 
00143   priv_nh_.param<double> ("clustering_voxel_size", clustering_voxel_size_,
00144       0.003);
00145 
00146   nh_.param<double> ("grad_weight", grad_weight_, 80.0);
00147 
00148   nh_.param<double> ("window_size", window_size_, 0.10);
00149 
00150   nh_.param<double> ("ball_size", ball_size_, 0.10);
00151 
00152   object_segmenter_ = new ObjectSegmenter(grad_weight_, n_iter_, use_gpu_,
00153       window_size_, ball_size_);
00154 
00155 }
00156 
00157 ObjectSegmentationRvizUI::~ObjectSegmentationRvizUI()
00158 {
00159   if (object_segmentation_server_)
00160     stopActionServer();
00161 
00162   render_panel_->getRenderWindow()->setActive(false);
00163   delete render_panel_;
00164   delete image_overlay_;
00165   delete object_segmenter_;
00166 }
00167 
00168 void ObjectSegmentationRvizUI::createRenderPanel(
00169     rviz::VisualizationManager *visualization_manager )
00170 {
00171   //put a render panel into the window
00172   render_panel_ = new rviz::RenderPanel(this, false);
00173 
00174   render_panel_->SetSize(m_panel->GetRect());
00175 
00176   // notify us when the user does something with the mouse
00177   render_panel_->Connect(
00178       wxEVT_MOTION,
00179       wxMouseEventHandler( ObjectSegmentationRvizUI::onRenderWindowMouseEvents ),
00180       NULL, this);
00181   render_panel_->Connect(
00182       wxEVT_LEFT_DOWN,
00183       wxMouseEventHandler( ObjectSegmentationRvizUI::onRenderWindowMouseEvents ),
00184       NULL, this);
00185   render_panel_->Connect(
00186       wxEVT_MIDDLE_DOWN,
00187       wxMouseEventHandler( ObjectSegmentationRvizUI::onRenderWindowMouseEvents ),
00188       NULL, this);
00189   render_panel_->Connect(
00190       wxEVT_RIGHT_DOWN,
00191       wxMouseEventHandler( ObjectSegmentationRvizUI::onRenderWindowMouseEvents ),
00192       NULL, this);
00193   render_panel_->Connect(
00194       wxEVT_LEFT_UP,
00195       wxMouseEventHandler( ObjectSegmentationRvizUI::onRenderWindowMouseEvents ),
00196       NULL, this);
00197   render_panel_->Connect(
00198       wxEVT_MIDDLE_UP,
00199       wxMouseEventHandler( ObjectSegmentationRvizUI::onRenderWindowMouseEvents ),
00200       NULL, this);
00201   render_panel_->Connect(
00202       wxEVT_RIGHT_UP,
00203       wxMouseEventHandler( ObjectSegmentationRvizUI::onRenderWindowMouseEvents ),
00204       NULL, this);
00205   render_panel_->Connect(
00206       wxEVT_MOUSEWHEEL,
00207       wxMouseEventHandler( ObjectSegmentationRvizUI::onRenderWindowMouseEvents ),
00208       NULL, this);
00209   render_panel_->Connect(
00210       wxEVT_LEFT_DCLICK,
00211       wxMouseEventHandler( ObjectSegmentationRvizUI::onRenderWindowMouseEvents ),
00212       NULL, this);
00213 
00214   render_panel_->createRenderWindow();
00215   render_panel_->initialize(scene_manager_, visualization_manager);
00216 
00217   render_panel_->setAutoRender(false);
00218   render_panel_->getViewport()->setOverlaysEnabled(false);
00219   render_panel_->getViewport()->setClearEveryFrame(true);
00220   render_panel_->getRenderWindow()->setAutoUpdated(false);
00221   render_panel_->getRenderWindow()->setActive(true);
00222 
00223 }
00224 
00225 void ObjectSegmentationRvizUI::startActionServer( ros::NodeHandle &node_handle )
00226 {
00227   if (object_segmentation_server_)
00228   {
00229     ROS_ERROR( "ObjectSegmentationGuiAction server already started!" );
00230     return;
00231   }
00232 
00233   ROS_INFO("Starting ObjectSegmentationGuiAction server.");
00234 
00235   //create non-threaded action server
00236   object_segmentation_server_ = new actionlib::SimpleActionServer<
00237       ObjectSegmentationGuiAction>(node_handle, "segmentation_popup", false);
00238 
00239   object_segmentation_server_->registerGoalCallback(boost::bind(
00240       &ObjectSegmentationRvizUI::acceptNewGoal, this));
00241   object_segmentation_server_->registerPreemptCallback(boost::bind(
00242       &ObjectSegmentationRvizUI::preempt, this));
00243 
00244   object_segmentation_server_->start();
00245 }
00246 
00247 void ObjectSegmentationRvizUI::stopActionServer()
00248 {
00249   if (!object_segmentation_server_)
00250   {
00251     ROS_ERROR("ObjectSegmentationGuiAction server cannot be stopped because it is not running.");
00252     return;
00253   }
00254 
00255   //if we're currently being active, we have to cancel everything, clean up & hide the window
00256   if (object_segmentation_server_->isActive())
00257   {
00258     ROS_WARN("Aborting ObjectSegmentationGuiAction goal.");
00259     stopSegmentation();
00260     object_segmentation_server_->setAborted();
00261     cleanupAndHide();
00262   }
00263 
00264   ROS_INFO("Stopping ObjectSegmentationGuiAction server.");
00265   delete object_segmentation_server_;
00266   object_segmentation_server_ = 0;
00267 }
00268 
00269 void ObjectSegmentationRvizUI::acceptNewGoal()
00270 {
00271 
00272   // only if segmentation is running labeling can be accepted
00273   accept_button_->Enable(running_);
00274 
00275   // only if segmentation has been run at least once,
00276   // same seed point cloud be reused
00277   restart_button_->Enable(!previous_queue_.empty());
00278 
00279   // only if segmentation is not running, it can be started
00280   segment_button_->Enable(!running_);
00281 
00282   const ObjectSegmentationGuiGoal::ConstPtr &goal =
00283       object_segmentation_server_->acceptNewGoal();
00284 
00285   rviz_interaction_tools::updateCamera(render_panel_->getCamera(),
00286       goal->camera_info);
00287 
00288   ROS_ASSERT( goal->point_cloud.height ==
00289       goal->disparity_image.image.height &&
00290       goal->point_cloud.width ==
00291       goal->disparity_image.image.width);
00292 
00293   current_point_cloud_ = goal->point_cloud;
00294   current_camera_info_ = goal->camera_info;
00295   current_disparity_image_ = goal->disparity_image;
00296   fillRgbImage(current_image_, current_point_cloud_);
00297   //    image_pub_.publish(current_image_);
00298 
00299   float baseline = current_disparity_image_.T;
00300   table_transformer_.setParams(current_camera_info_, baseline, up_direction_);
00301 
00302   //store data for later use in update()
00303   //we can't directly put in in ogre because
00304   //this runs in a different thread
00305   image_overlay_->setImage(current_image_);
00306   image_overlay_->update();
00307 
00308   Show();
00309 
00310   click_info_.down_x_ = click_info_.down_y_ = 0;
00311 
00312   // initialise segmentation
00313   object_segmenter_->setNewData(current_image_, current_disparity_image_);
00314 
00315   // set checkboxes accordingly
00316   bool with_colors, with_color_holes, uniform, with_disparities, with_surface;
00317   object_segmenter_->getCurrentSegmFlags(with_colors, with_color_holes,
00318       uniform, with_disparities, with_surface);
00319 
00320   with_surface_->SetValue(with_surface);
00321   with_surface_->Enable(true);
00322   with_disparity_->SetValue(with_disparities);
00323   with_disparity_->Enable(true);
00324   with_color_->SetValue(with_colors);
00325   with_color_->Enable(true);
00326   with_holes_->SetValue(with_color_holes);
00327   with_holes_->Enable(true);
00328   uniform_->SetValue(uniform);
00329   uniform_->Enable(true);
00330 
00331   grad_weight_slider_->SetValue(grad_weight_);
00332 
00333   initStorage(current_image_);
00334 
00335 }
00336 
00337 void ObjectSegmentationRvizUI::initStorage( const sensor_msgs::Image &image )
00338 {
00339   // initialise mask container
00340   inits_.header.frame_id = "narrow_stereo_optical_frame";
00341   inits_.header.stamp = ros::Time::now();
00342   inits_.height = image.height;
00343   inits_.width = image.width;
00344   inits_.encoding = enc::MONO8;
00345   inits_.is_bigendian = false;
00346   inits_.step = image.width;
00347   size_t size = inits_.step * inits_.height;
00348   inits_.data.resize(size);
00349 
00350   texture_buffer_.header = image.header;
00351   texture_buffer_.height = image.height;
00352   texture_buffer_.width = image.width;
00353   texture_buffer_.encoding = enc::RGB8;
00354   texture_buffer_.is_bigendian = false;
00355   texture_buffer_.step = 3 * texture_buffer_.width;
00356   size = texture_buffer_.step * texture_buffer_.height;
00357   texture_buffer_.data.resize(size);
00358 }
00359 
00360 void ObjectSegmentationRvizUI::preempt()
00361 {
00362   stopSegmentation();
00363   object_segmentation_server_->setPreempted();
00364   cleanupAndHide();
00365 }
00366 
00367 void ObjectSegmentationRvizUI::cleanupAndHide()
00368 {
00369   image_overlay_->clear();
00370   segment_button_->SetLabel(wxT("Segment"));
00371   paused_ = true;
00372   resetVars();
00373   previous_queue_.clear();
00374   Hide();
00375 }
00376 
00377 void ObjectSegmentationRvizUI::stopSegmentation()
00378 {
00379   ObjectSegmenter::Action action;
00380   action.type_ = ObjectSegmenter::STOP;
00381   object_segmenter_->queueAction(action);
00382 }
00383 
00384 void ObjectSegmentationRvizUI::resetVars()
00385 {
00386   for (size_t i = 0; i < clusters_.size(); ++i)
00387     clusters_[i].points.clear();
00388 
00389   clusters_.clear();
00390   segm_size_.clear();
00391   table_points_.points.clear();
00392   num_fg_hypos_ = 0;
00393   color_code_.clear();
00394 
00395   size_t size = inits_.step * inits_.height;
00396   inits_.data.clear();
00397   inits_.data.resize(size, 0);
00398 
00399   labels_.data.clear();
00400   labels_.data.resize(size, 0);
00401 
00402   image_overlay_->setImage(current_image_);
00403   image_overlay_->update();
00404 
00405   region_queue_.clear();
00406   //previous_queue_.clear();
00407 
00408   running_ = false;
00409   paused_ = true;
00410 
00411   // only if segmentation is running labeling can be accepted
00412   accept_button_->Enable(running_);
00413 
00414   // only if segmentation has been run at least once,
00415   // same seed point cloud be reused
00416   restart_button_->Enable(!previous_queue_.empty());
00417 
00418   // only if segmentation is not running, it can be started
00419   segment_button_->Enable(!running_);
00420 }
00421 
00422 void ObjectSegmentationRvizUI::update( float wall_dt, float ros_dt )
00423 {
00424   render_panel_->getRenderWindow()->update();
00425 
00426   // if button pressed
00427   if (click_info_.down_)
00428     updateSelectBox(click_info_.down_x_, click_info_.down_y_, click_info_.x_,
00429         click_info_.y_);
00430   else
00431     updateSelectBox(0.0, 0.0, 0.0, 0.0);
00432 
00433   if (num_fg_hypos_ > 0)
00434   {
00435     if (running_)
00436     {
00437       object_segmenter_->getCurrentResult(labels_);
00438       /*
00439        ROS_INFO("Sum of labels %d given %d foreground hypos",
00440        sumLabels(labels_), num_fg_hypos_);
00441        */
00442       //        image_pub_.publish(labels_);
00443     }
00444     overlaySegmentationMask();
00445   }
00446 
00447   // only if segmentation has been run at least once,
00448   // same seed point cloud be reused
00449   restart_button_->Enable(!previous_queue_.empty());
00450 }
00451 
00452 int ObjectSegmentationRvizUI::sumLabels( const sensor_msgs::Image &labels )
00453 {
00454   int sum = 0;
00455   for (unsigned int i = 0; i < labels.width; ++i)
00456     for (unsigned int j = 0; j < labels.height; ++j)
00457     {
00458       int adr = j * labels.width + i;
00459       sum += labels.data[adr];
00460     }
00461 
00462   return sum;
00463 }
00464 
00465 void ObjectSegmentationRvizUI::onRenderWindowMouseEvents( wxMouseEvent& event )
00466 {
00467   int x = event.GetX();
00468   int y = event.GetY();
00469   //convert to coordinates in the original image resolution
00470   wxSize size = render_panel_->GetSize();
00471   x = floor(x * current_image_.width / size.GetWidth());
00472   y = floor(y * current_image_.height / size.GetHeight());
00473 
00474   if (event.ButtonDown(wxMOUSE_BTN_LEFT))
00475   {
00476     click_info_.down_ = true;
00477 
00478     ROS_DEBUG("Good click at (%d,%d)", x, y);
00479     click_info_.down_x_ = x;
00480     click_info_.down_y_ = y;
00481 
00482     click_info_.x_ = x;
00483     click_info_.y_ = y;
00484 
00485   }
00486 
00487   if (event.ButtonUp(wxMOUSE_BTN_LEFT))
00488   {
00489 
00490     if (click_info_.down_x_ > click_info_.x_)
00491       swap(click_info_.down_x_, click_info_.x_);
00492 
00493     if (click_info_.down_y_ > click_info_.y_)
00494       swap(click_info_.down_y_, click_info_.y_);
00495 
00496     float dist_clicks = dist(click_info_.down_x_, click_info_.down_y_,
00497         click_info_.x_, click_info_.y_);
00498 
00499     ObjectSegmenter::Action action;
00500 
00501     Point tl;
00502     tl.x = click_info_.down_x_;
00503     tl.y = click_info_.down_y_;
00504 
00505     Point br;
00506 
00507     if (click_info_.dragged_ && dist_clicks > 2)
00508     {
00509 
00510       br.x = click_info_.x_;
00511       br.y = click_info_.y_;
00512 
00513       click_info_.dragged_ = false;
00514       action.type_ = ObjectSegmenter::REGION;
00515 
00516     } else
00517     {
00518 
00519       br.x = -1;
00520       br.y = -1;
00521 
00522       click_info_.dragged_ = false;
00523       action.type_ = ObjectSegmenter::CLICK;
00524     }
00525 
00526     ObjectSegmenter::Box2D region;
00527     region.p_tl_ = tl;
00528     region.p_br_ = br;
00529 
00530     if (!running_ && region_queue_.size() < MAX_FG)
00531     {
00532       region_queue_.push_back(region);
00533       previous_queue_.push_back(region);
00534       addToMasks(region);
00535       num_fg_hypos_++;
00536       addColorCode();
00537     } else if (num_fg_hypos_ < MAX_FG)
00538     {
00539       previous_queue_.push_back(region);
00540       action.box_ = region;
00541       object_segmenter_->queueAction(action);
00542       num_fg_hypos_++;
00543       addColorCode();
00544     }
00545 else      ROS_WARN("Maximum Number of Segments reached.");
00546 
00547       click_info_.down_ = false;
00548 
00549     }
00550 
00551     if (event.Dragging())
00552     {
00553       if (click_info_.down_)
00554       {
00555         click_info_.dragged_ = true;
00556 
00557         click_info_.x_ = x;
00558         click_info_.y_ = y;
00559       }
00560     }
00561   }
00562 
00563   void ObjectSegmentationRvizUI::addColorCode()
00564   {
00565     // Generate random colour for segment
00566 
00567     int zero = rand()&2;
00568     for(int i=0; i<3; i++)
00569     if(i==zero)
00570     color_code_.push_back(0);
00571     else
00572     color_code_.push_back(rand()&255);
00573   }
00574 
00575   void ObjectSegmentationRvizUI::addToMasks(const ObjectSegmenter::Box2D &select_)
00576   {
00577 
00578     int w = inits_.width;
00579 
00580     int size = 0;
00581 
00582     // region selected 
00583     if(!(select_.p_br_.x == -1 && select_.p_br_.y == -1))
00584     {
00585 
00586       for(int y=select_.p_tl_.y; y<select_.p_br_.y; ++y)
00587       {
00588         for(int x=select_.p_tl_.x; x<select_.p_br_.x; ++x)
00589         {
00590           int i = y*w + x;
00591           inits_.data[i]=num_fg_hypos_ + 2; // the same as the labels from the segmentation
00592           size ++;
00593         }
00594       }
00595 
00596       ROS_DEBUG("Added rectangle of size %d", size);
00597 
00598     } else
00599     {
00600       // point selected - region initialisation done automatically
00601 
00602       int radius = 10;
00603 
00604       for(int y=select_.p_tl_.y-radius; y<select_.p_tl_.y+radius; ++y)
00605       {
00606         for(int x=select_.p_tl_.x-radius; x<select_.p_tl_.x+radius; ++x)
00607         {
00608 
00609           if(dist((int)select_.p_tl_.x, (int)select_.p_tl_.y, x, y)<=radius)
00610           {
00611             int i = y*w + x;
00612             inits_.data[i]=num_fg_hypos_ + 2;
00613             size++;
00614           }
00615         }
00616       }
00617 
00618       ROS_DEBUG("Added circle of size %d", size);
00619     }
00620   }
00621 
00622   void ObjectSegmentationRvizUI::fillRgbImage(sensor_msgs::Image &rgb_img,
00623       const sensor_msgs::PointCloud2 &point_cloud)
00624   {
00625 
00626     ROS_DEBUG("Width and Height: (%d %d)",point_cloud.height, point_cloud.width );
00627 
00628     rgb_img.header = point_cloud.header;
00629     rgb_img.height = point_cloud.height;
00630     rgb_img.width = point_cloud.width;
00631     rgb_img.encoding = enc::RGB8;
00632     rgb_img.is_bigendian = false;
00633     rgb_img.step = 3 * rgb_img.width;
00634     size_t size = rgb_img.step * rgb_img.height;
00635     rgb_img.data.resize(size);
00636 
00637     for(unsigned int x=0; x<rgb_img.width; ++x)
00638     {
00639       for(unsigned int y=0; y<rgb_img.height; ++y)
00640       {
00641         int i = y * rgb_img.width + x;
00642 
00643         float rgb;
00644         memcpy ( &rgb, &point_cloud.data[i * point_cloud.point_step + point_cloud.fields[3].offset], sizeof (float));
00645         float r, g, b;
00646         transformRgb(rgb, r, g, b);
00647 
00648         int wide_i = y * rgb_img.step + x*3;
00649         rgb_img.data[wide_i+0] = round(r*255.0f);
00650         rgb_img.data[wide_i+1] = round(g*255.0f);
00651         rgb_img.data[wide_i+2] = round(b*255.0f);
00652 
00653       }
00654     }
00655   }
00656 
00657   void ObjectSegmentationRvizUI::overlaySegmentationMask()
00658   {
00659 
00660     sensor_msgs::Image::ConstPtr overlay_ptr;
00661 
00662     if( !running_)
00663     {
00664       overlay_ptr = boost::make_shared<const sensor_msgs::Image> (inits_);
00665     } else
00666     {
00667       overlay_ptr = boost::make_shared<const sensor_msgs::Image> (labels_);
00668     }
00669 
00670     // decrease in gray shade for surface 
00671     //    uint8_t darker = 50;
00672 
00673     //copy image to a new buffer so we can paint the segmentation mask
00674 
00675     int count = 0;
00676 
00677     int w = current_image_.width;
00678     int h = current_image_.height;
00679 
00680     for (int i=0; i<h; i++)
00681     {
00682       for (int j=0; j<w; j++)
00683       {
00684         unsigned int adr = i *w + j;
00685         // foreground segments start at label 2, background=0, flat surface=1
00686         int label = (int)(overlay_ptr->data[adr]);
00687 
00688         for(int c=0; c<3; c++)
00689         {
00690           uint8_t val;
00691           // we want to color all foreground segments
00692           if(label>1)
00693           {
00694             // only one channel will be changed,
00695             // the other will have 0 and colour info has to be kept
00696             ROS_ASSERT(label-2<num_fg_hypos_);
00697             ROS_ASSERT(3 * (label-2) + c<(int)color_code_.size());
00698             if( color_code_.at(3 * (label-2) + c)==0)
00699             val = current_image_.data[3 * adr + c];
00700             else
00701             val = color_code_.at(3 * (label-2) + c);
00702 
00703             count ++;
00704 
00705           } /*else if(label==1){
00706            val = current_image_.data[3 * adr + c];
00707            val = val>darker ? val-darker : 0;
00708            }
00709            */
00710           else
00711           {
00712             val = current_image_.data[3 * adr + c];
00713           }
00714 
00715           texture_buffer_.data.at( 3 * adr + c ) = val;
00716         }
00717       }
00718     }
00719 
00720     image_overlay_->setImage(texture_buffer_);
00721     image_overlay_->update();
00722   }
00723 
00724   void ObjectSegmentationRvizUI::updateSelectBox(int start_x,
00725       int start_y,
00726       int stop_x,
00727       int stop_y)
00728   {
00729 
00730     if(start_x>stop_x)
00731     swap(start_x, stop_x);
00732 
00733     if(start_y>stop_y)
00734     swap(start_y, stop_y);
00735 
00736     float x0 = start_x / ((float)current_image_.width / 2.0 ) - 1.0;
00737     float y0 = - (start_y / ((float)current_image_.height / 2.0 ) - 1.0);
00738 
00739     float x1 = stop_x / ((float)current_image_.width / 2.0 ) - 1.0;
00740     float y1 = - (stop_y / ((float)current_image_.height / 2.0 ) - 1.0);
00741 
00742     box_object_->beginUpdate(0);
00743     box_object_->position(x0, y0, 0.0);
00744     box_object_->position(x0, y1, 0.0);
00745     box_object_->position(x1, y1, 0.0);
00746     box_object_->position(x1, y0, 0.0);
00747     box_object_->index(0);
00748     box_object_->index(1);
00749     box_object_->index(2);
00750     box_object_->index(3);
00751     box_object_->index(0);
00752     box_object_->end();
00753   }
00754 
00755   void ObjectSegmentationRvizUI::acceptButtonClicked(wxCommandEvent&)
00756   {
00757     if( !running_) return;
00758 
00759     ObjectSegmentationGuiResult result;
00760 
00761     while(!object_segmenter_->getCurrentResult( labels_))
00762     {
00763       ROS_INFO("Waiting for final labeling");
00764     }
00765     float alpha, beta, gamma;
00766     object_segmenter_->getCurrentSurface( alpha, beta, gamma);
00767 
00768     // stop segmentation thread
00769     stopSegmentation();
00770 
00771     getClusterSize();
00772 
00773     if ( segm_size_[0] < (int)inlier_threshold_)
00774     {
00775       ROS_INFO("Plane detection has %d inliers, below min threshold of %d",
00776           segm_size_[0],
00777           inlier_threshold_);
00778       result.result = result.NO_TABLE;
00779       return;
00780     }
00781 
00782     for(int i=1; i<(int)segm_size_.size(); ++i)
00783     if(segm_size_[i] == 0)
00784     {
00785       ROS_INFO("Segment %d has 0 points.", segm_size_[i]);
00786       num_fg_hypos_--;
00787     }
00788 
00789     reconstructAndClusterPointCloud(result);
00790 
00791     // get table parameters in 3d
00792     tabletop_object_detector::Table table = table_transformer_.get3DTable(alpha, beta, gamma, table_points_,
00793         clusters_[0].header);
00794     result.table = table;
00795 
00796     result.result = result.SUCCESS;
00797 
00798     ROS_INFO("ObjectSegmentation was successful.");
00799 
00800     object_segmentation_server_->setSucceeded(result);
00801 
00802     cleanupAndHide();
00803   }
00804 
00805   void ObjectSegmentationRvizUI::cancelButtonClicked(wxCommandEvent&)
00806   {
00807     stopSegmentation();
00808     object_segmentation_server_->setAborted();
00809     cleanupAndHide();
00810   }
00811 
00812   void ObjectSegmentationRvizUI::resetButtonClicked( wxCommandEvent& )
00813   {
00814     reset( );
00815     previous_queue_.clear();
00816   }
00817 
00818   void ObjectSegmentationRvizUI::reset( )
00819   {
00820     ObjectSegmenter::Action action;
00821     action.type_ = ObjectSegmenter::RESET;
00822     includeFlags(action);
00823     object_segmenter_->queueAction(action);
00824     image_overlay_->clear();
00825 
00826     segment_button_->SetLabel(wxT("Segment"));
00827     paused_ = true;
00828 
00829     resetVars();
00830   }
00831 
00832   void ObjectSegmentationRvizUI::restartButtonClicked( wxCommandEvent& event )
00833   {
00834     // copy old seeds into new queue
00835     reset();
00836     // ObjectSegmenter::Action action;
00837     // action.type_ = ObjectSegmenter::RESET;
00838     // includeFlags(action);
00839     // object_segmenter_->queueAction(action);
00840     // image_overlay_->clear();
00841 
00842 
00843     // region_queue_.clear();
00844 
00845     for(int i=0; i<(int)previous_queue_.size();++i)
00846     {
00847       if( num_fg_hypos_ < MAX_FG )
00848       {
00849         region_queue_.push_back(previous_queue_[i]);
00850         addToMasks(previous_queue_[i]);
00851         num_fg_hypos_++;
00852         addColorCode();
00853       } else
00854       {
00855         ROS_INFO("Maximum number of segments reached");
00856       }
00857     }
00858 
00859     segment();
00860     //    std::string pause("Pause");
00861     segment_button_->SetLabel(wxT("Pause"));
00862     paused_ = false;
00863   }
00864 
00865   void ObjectSegmentationRvizUI::includeFlags( ObjectSegmenter::Action &ac)
00866   {
00867     // keep the current segmentation flags
00868     ac.with_colors_ = with_color_->GetValue();
00869     ac.with_color_holes_ = with_holes_->GetValue();
00870     ac.uniform_ = uniform_->GetValue();
00871     ac.with_disparities_ = with_disparity_->GetValue();
00872     ac.with_surface_ = with_surface_->GetValue();
00873 
00874   }
00875 
00876   void ObjectSegmentationRvizUI::segmentButtonClicked( wxCommandEvent& )
00877   {
00878     if(paused_)
00879     {
00880       segment();
00881       segment_button_->SetLabel(wxT("Pause"));
00882       paused_ = false;
00883     } else
00884     {
00885       // copy old seeds into new queue
00886       ObjectSegmenter::Action action;
00887       action.type_ = ObjectSegmenter::PAUSE;
00888       object_segmenter_->queueAction(action);
00889       segment_button_->SetLabel(wxT("Segment"));
00890       paused_ = true;
00891     }
00892   }
00893 
00894   void ObjectSegmentationRvizUI::segment()
00895   {
00896 
00897     if(region_queue_.empty())
00898     {
00899       // no region has been selected or click executed
00900       ObjectSegmenter::Action action;
00901       ObjectSegmenter::Box2D dummy;
00902       dummy.p_tl_.x = -1;
00903       dummy.p_tl_.y = -1;
00904       dummy.p_br_.x = -1;
00905       dummy.p_br_.y = -1;
00906       action.type_ = ObjectSegmenter::CLICK;
00907       action.box_ = dummy;
00908       object_segmenter_->queueAction(action);
00909 
00910     } else
00911     {
00912       while(!region_queue_.empty())
00913       {
00914         ObjectSegmenter::Action action;
00915         if(region_queue_[0].p_br_.x==-1 && region_queue_[0].p_br_.y==-1)
00916         {
00917           action.type_ = ObjectSegmenter::CLICK;
00918         } else
00919         {
00920           action.type_ = ObjectSegmenter::REGION;
00921         }
00922 
00923         action.box_ = region_queue_[0];
00924 
00925         object_segmenter_->queueAction(action);
00926         region_queue_.pop_front();
00927       }
00928     }
00929 
00930     running_=true;
00931     // only if segmentation is running labeling can be accepted
00932     accept_button_->Enable(running_);
00933 
00934     // only if segmentation has been run at least once, 
00935     // same seed point cloud be reused
00936     restart_button_->Enable(!previous_queue_.empty());
00937 
00938   }
00939 
00940   void ObjectSegmentationRvizUI::withSurfaceChecked( wxCommandEvent& event )
00941   { reset();}
00942 
00943   void ObjectSegmentationRvizUI::withDisparityChecked( wxCommandEvent& event )
00944   {
00945     if(!with_disparity_->IsChecked())
00946     {
00947       with_color_->SetValue(true);
00948       with_holes_->Enable(true);
00949       uniform_->Enable(true);
00950     }
00951     reset();
00952   }
00953 
00954   void ObjectSegmentationRvizUI::withColorChecked( wxCommandEvent& event )
00955   {
00956     if(!with_color_->IsChecked())
00957     {
00958       with_disparity_->SetValue(true);
00959       with_holes_->Enable(false);
00960       uniform_->Enable(false);
00961     } else
00962     {
00963       with_holes_->Enable(true);
00964       uniform_->Enable(true);
00965     }
00966 
00967     reset();
00968   }
00969 
00970   void ObjectSegmentationRvizUI::withHolesChecked( wxCommandEvent& event )
00971   { reset();}
00972 
00973   void ObjectSegmentationRvizUI::uniformChecked( wxCommandEvent& event )
00974   { reset();}
00975 
00976   void ObjectSegmentationRvizUI::gradWeightChanged( wxScrollEvent& event )
00977   {
00978     grad_weight_ = grad_weight_slider_->GetValue();
00979     ObjectSegmenter::Action action;
00980     action.type_ = ObjectSegmenter::GRADCHANGE;
00981     action.grad_weight_ = grad_weight_;
00982     object_segmenter_->queueAction(action);
00983   }
00984 
00985   void ObjectSegmentationRvizUI::getClusterSize()
00986   {
00987     int w = labels_.width;
00988     int h = labels_.height;
00989 
00990     // take into account surface and foreground hypotheses
00991     segm_size_.resize(num_fg_hypos_+1,0);
00992 
00993     for (int i=0; i<h; i++)
00994     {
00995       for (int j=0; j<w; j++)
00996       {
00997 
00998         unsigned int adr = i *w + j;
00999         unsigned int label = labels_.data[adr];
01000         // take foreground objects
01001         if(rviz_interaction_tools::hasDisparityValue(current_disparity_image_,
01002                 i, j))
01003         {
01004           if(label>1)
01005           segm_size_.at(label-1)++;
01006           else if(label==1)
01007           segm_size_.at(0)++;
01008         }
01009       }
01010     }
01011   }
01012 
01013   void ObjectSegmentationRvizUI::reconstructAndClusterPointCloud( ObjectSegmentationGuiResult &result)
01014   {
01015     clusters_.clear();
01016     clusters_.resize(num_fg_hypos_);
01017     for(int i=0; i<num_fg_hypos_; ++i)
01018     {
01019       clusters_[i].header.frame_id = current_point_cloud_.header.frame_id;
01020       clusters_[i].header.stamp = ros::Time::now();
01021       clusters_[i].points.reserve(segm_size_[i+1]);
01022     }
01023 
01024     table_points_.header.frame_id = current_point_cloud_.header.frame_id;
01025     table_points_.header.stamp = ros::Time::now();
01026     table_points_.points.reserve(segm_size_[0]);
01027 
01028     int nan_cluster =0;
01029 
01030     for(unsigned int x=0; x<current_point_cloud_.width; ++x)
01031     {
01032       for(unsigned int y=0; y<current_point_cloud_.height; ++y)
01033       {
01034         int i = y * current_point_cloud_.width + x;
01035         float nan;
01036         memcpy (&nan,
01037             &current_point_cloud_.data[i * current_point_cloud_.point_step + current_point_cloud_.fields[0].offset],
01038             sizeof (float));
01039         if(!isnan(nan))
01040         {
01041           geometry_msgs::Point32 p;
01042           float x, y, z;
01043           memcpy (&x,
01044               &current_point_cloud_.data[i
01045               * current_point_cloud_.point_step
01046               + current_point_cloud_.fields[0].offset],
01047               sizeof (float));
01048           memcpy (&y,
01049               &current_point_cloud_.data[i
01050               * current_point_cloud_.point_step
01051               + current_point_cloud_.fields[1].offset],
01052               sizeof (float));
01053           memcpy (&z,
01054               &current_point_cloud_.data[i
01055               * current_point_cloud_.point_step
01056               + current_point_cloud_.fields[2].offset],
01057               sizeof (float));
01058           p.x = x;
01059           p.y = y;
01060           p.z = z;
01061 
01062           unsigned int label = labels_.data[i];
01063           if(label>1)
01064           {
01065             clusters_[label-2].points.push_back( p );
01066           } else if(label==1)
01067           {
01068             table_points_.points.push_back( p );
01069           }
01070         } else
01071         {
01072           nan_cluster++;
01073         }
01074       }
01075     }
01076 
01077     filterOutliersAndDownsample(clusters_);
01078 
01079     result.clusters = clusters_;
01080   }
01081 
01082   void ObjectSegmentationRvizUI::filterOutliersAndDownsample(std::vector<sensor_msgs::PointCloud> &clusters)
01083   {
01084     std::vector<sensor_msgs::PointCloud> filtered_clusters;
01085     filtered_clusters.reserve(clusters.size());
01086 
01087     for (size_t i=0; i<clusters.size(); i++)
01088     {
01089 
01090       // annoying conversion to different point cloud formats
01091       sensor_msgs::PointCloud2 converted_cloud;
01092       sensor_msgs::convertPointCloudToPointCloud2(clusters[i], converted_cloud);
01093 
01094       pcl::PointCloud<PCL_Point> cloud_t;
01095       pcl::fromROSMsg (converted_cloud, cloud_t);
01096       pcl::PointCloud<PCL_Point>::ConstPtr cloud_ptr
01097       = boost::make_shared<const pcl::PointCloud<PCL_Point> > (cloud_t);
01098 
01099       // statistical outlier removal
01100       pcl::StatisticalOutlierRemoval<PCL_Point> sor;
01101       sor.setInputCloud (cloud_ptr);
01102       sor.setMeanK (mean_k_);
01103       sor.setStddevMulThresh (std_);
01104       pcl::PointCloud<PCL_Point>::Ptr
01105       cloud_filtered(new pcl::PointCloud<PCL_Point> ());
01106       sor.filter (*cloud_filtered);
01107 
01108       // and annoying back conversion into old point cloud format
01109       sensor_msgs::PointCloud2 filt_converted_cloud;
01110       pcl::toROSMsg ( *cloud_filtered, filt_converted_cloud);
01111       sensor_msgs::PointCloud2::ConstPtr filt_converted_cloud_ptr
01112       = boost::make_shared<const sensor_msgs::PointCloud2> (filt_converted_cloud);
01113 
01114       // downsampling of point clouds
01115       pcl::VoxelGrid<sensor_msgs::PointCloud2> dvx;
01116       dvx.setInputCloud(filt_converted_cloud_ptr);
01117       dvx.setLeafSize (clustering_voxel_size_,
01118           clustering_voxel_size_,
01119           clustering_voxel_size_);
01120       sensor_msgs::PointCloud2::Ptr
01121       cloud_downsampled(new sensor_msgs::PointCloud2());
01122       dvx.filter (*cloud_downsampled);
01123 
01124       sensor_msgs::PointCloud cloud_tmp;
01125       sensor_msgs::convertPointCloud2ToPointCloud(*cloud_downsampled, cloud_tmp);
01126 
01127       filtered_clusters.push_back(cloud_tmp);
01128     }
01129 
01130     clusters.clear();
01131     clusters = filtered_clusters;
01132 
01133   }
01134 
01135 } // namespace object_segmentation_gui
01136
object_segmentation_rviz_ui.cpp
