コード例 #1
0
ファイル: image_viewer.hpp プロジェクト: TuZZiX/ROS_IDE_inc
template <typename T> bool
pcl::visualization::ImageViewer::addMask (
    const typename pcl::PointCloud<T>::ConstPtr &image,
    const pcl::PointCloud<T> &mask, 
    double r, double g, double b,
    const std::string &layer_id, double opacity)
{
  // We assume that the data passed into image is organized, otherwise this doesn't make sense
  if (!image->isOrganized ())
    return (false);

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addMask] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, image_viewer_->GetRenderWindow ()->GetSize ()[0] - 1, image_viewer_->GetRenderWindow ()->GetSize ()[1] - 1, opacity, true);
  }

  am_it->canvas->SetDrawColor (r * 255.0, g * 255.0, b * 255.0, opacity * 255.0);

  // Construct a search object to get the camera parameters
  pcl::search::OrganizedNeighbor<T> search;
  search.setInputCloud (image);
  for (size_t i = 0; i < mask.points.size (); ++i)
  {
    pcl::PointXY p_projected;
    search.projectPoint (mask.points[i], p_projected);

    am_it->canvas->DrawPoint (int (p_projected.x), 
                              int (float (image->height) - p_projected.y));
  }

  return (true);
}
コード例 #2
0
ファイル: image_viewer.hpp プロジェクト: gpicchiarelli/pcl
template <typename T> bool
pcl::visualization::ImageViewer::addRectangle (
    const typename pcl::PointCloud<T>::ConstPtr &image,
    const pcl::PointCloud<T> &mask, 
    double r, double g, double b,
    const std::string &layer_id, double opacity)
{
  // We assume that the data passed into image is organized, otherwise this doesn't make sense
  if (!image->isOrganized ())
    return (false);

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addRectangle] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, true);
  }

  // Construct a search object to get the camera parameters
  pcl::search::OrganizedNeighbor<T> search;
  search.setInputCloud (image);
  std::vector<pcl::PointXY> pp_2d (mask.points.size ());
  for (size_t i = 0; i < mask.points.size (); ++i)
    search.projectPoint (mask.points[i], pp_2d[i]);

  pcl::PointXY min_pt_2d, max_pt_2d;
  min_pt_2d.x = min_pt_2d.y = std::numeric_limits<float>::max ();
  max_pt_2d.x = max_pt_2d.y = std::numeric_limits<float>::min ();
  // Search for the two extrema
  for (size_t i = 0; i < pp_2d.size (); ++i)
  {
    if (pp_2d[i].x < min_pt_2d.x) min_pt_2d.x = pp_2d[i].x;
    if (pp_2d[i].y < min_pt_2d.y) min_pt_2d.y = pp_2d[i].y;
    if (pp_2d[i].x > max_pt_2d.x) max_pt_2d.x = pp_2d[i].x;
    if (pp_2d[i].y > max_pt_2d.y) max_pt_2d.y = pp_2d[i].y;
  }
#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION < 10))
  min_pt_2d.y = float (image->height) - min_pt_2d.y;
  max_pt_2d.y = float (image->height) - max_pt_2d.y;
#endif

  vtkSmartPointer<context_items::Rectangle> rect = vtkSmartPointer<context_items::Rectangle>::New ();
  rect->setColors (static_cast<unsigned char> (255.0 * r), 
                   static_cast<unsigned char> (255.0 * g), 
                   static_cast<unsigned char> (255.0 * b));
  rect->setOpacity (opacity);
  rect->set (min_pt_2d.x, min_pt_2d.y, (max_pt_2d.x - min_pt_2d.x), (max_pt_2d.y - min_pt_2d.y));
  am_it->actor->GetScene ()->AddItem (rect);

  return (true);
}
コード例 #3
0
ファイル: image_viewer.hpp プロジェクト: gpicchiarelli/pcl
template <typename T> bool
pcl::visualization::ImageViewer::addPlanarPolygon (
    const typename pcl::PointCloud<T>::ConstPtr &image,
    const pcl::PlanarPolygon<T> &polygon, 
    double r, double g, double b,
    const std::string &layer_id, double opacity)
{
  // We assume that the data passed into image is organized, otherwise this doesn't make sense
  if (!image->isOrganized ())
    return (false);

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addPlanarPolygon] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, true);
  }
  
  // Construct a search object to get the camera parameters and fill points
  pcl::search::OrganizedNeighbor<T> search;
  search.setInputCloud (image);
  const float image_height_f = static_cast<float> (image->height);
  std::vector<float> xy;
  xy.reserve ((polygon.getContour ().size () + 1) * 2);
  for (size_t i = 0; i < polygon.getContour ().size (); ++i)
  {
    pcl::PointXY p;
    search.projectPoint (polygon.getContour ()[i], p);
    xy.push_back (p.x);
    #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION < 10))
    xy.push_back (image_height_f - p.y);
    #else
    xy.push_back (p.y);
    #endif
  }

  // Close the polygon
  xy[xy.size () - 2] = xy[0];
  xy[xy.size () - 1] = xy[1];

  vtkSmartPointer<context_items::Polygon> poly = vtkSmartPointer<context_items::Polygon>::New ();
  poly->setColors (static_cast<unsigned char> (r * 255.0), 
                   static_cast<unsigned char> (g * 255.0), 
                   static_cast<unsigned char> (b * 255.0));
  poly->set (xy);
  am_it->actor->GetScene ()->AddItem (poly);

  return (true);
}
コード例 #4
0
ファイル: image_viewer.hpp プロジェクト: TuZZiX/ROS_IDE_inc
template <typename T> bool
pcl::visualization::ImageViewer::addRectangle (
    const typename pcl::PointCloud<T>::ConstPtr &image,
    const pcl::PointCloud<T> &mask, 
    const std::string &layer_id, double opacity)
{
  // We assume that the data passed into image is organized, otherwise this doesn't make sense
  if (!image->isOrganized ())
    return (false);

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addRectangle] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, image_viewer_->GetRenderWindow ()->GetSize ()[0] - 1, image_viewer_->GetRenderWindow ()->GetSize ()[1] - 1, opacity, true);
  }

  am_it->canvas->SetDrawColor (0.0, 255.0, 0.0, opacity * 255.0);

  // Construct a search object to get the camera parameters
  pcl::search::OrganizedNeighbor<T> search;
  search.setInputCloud (image);
  std::vector<pcl::PointXY> pp_2d (mask.points.size ());
  for (size_t i = 0; i < mask.points.size (); ++i)
    search.projectPoint (mask.points[i], pp_2d[i]);

  pcl::PointXY min_pt_2d, max_pt_2d;
  min_pt_2d.x = min_pt_2d.y = std::numeric_limits<float>::max ();
  max_pt_2d.x = max_pt_2d.y = std::numeric_limits<float>::min ();
  // Search for the two extrema
  for (size_t i = 0; i < pp_2d.size (); ++i)
  {
    if (pp_2d[i].x < min_pt_2d.x) min_pt_2d.x = pp_2d[i].x;
    if (pp_2d[i].y < min_pt_2d.y) min_pt_2d.y = pp_2d[i].y;
    if (pp_2d[i].x > max_pt_2d.x) max_pt_2d.x = pp_2d[i].x;
    if (pp_2d[i].y > max_pt_2d.y) max_pt_2d.y = pp_2d[i].y;
  }
  min_pt_2d.y = float (image->height) - min_pt_2d.y;
  max_pt_2d.y = float (image->height) - max_pt_2d.y;

  am_it->canvas->DrawSegment (int (min_pt_2d.x), int (min_pt_2d.y), int (min_pt_2d.x), int (max_pt_2d.y));
  am_it->canvas->DrawSegment (int (min_pt_2d.x), int (max_pt_2d.y), int (max_pt_2d.x), int (max_pt_2d.y));
  am_it->canvas->DrawSegment (int (max_pt_2d.x), int (max_pt_2d.y), int (max_pt_2d.x), int (min_pt_2d.y));
  am_it->canvas->DrawSegment (int (max_pt_2d.x), int (min_pt_2d.y), int (min_pt_2d.x), int (min_pt_2d.y));


  return (true);
}
コード例 #5
0
ファイル: image_viewer.hpp プロジェクト: BITVoyager/pcl
template <typename T> bool
pcl::visualization::ImageViewer::addMask (
    const typename pcl::PointCloud<T>::ConstPtr &image,
    const pcl::PointCloud<T> &mask, 
    double r, double g, double b,
    const std::string &layer_id, double opacity)
{
  // We assume that the data passed into image is organized, otherwise this doesn't make sense
  if (!image->isOrganized ())
    return (false);

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addMask] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, false);
  }

  // Construct a search object to get the camera parameters
  pcl::search::OrganizedNeighbor<T> search;
  search.setInputCloud (image);
  std::vector<float> xy;
  xy.reserve (mask.size () * 2);
  for (size_t i = 0; i < mask.size (); ++i)
  {
    pcl::PointXY p_projected;
    search.projectPoint (mask[i], p_projected);

    xy.push_back (p_projected.x);
    xy.push_back (static_cast<float> (image->height) - p_projected.y);
  }

  vtkSmartPointer<context_items::Points> points = vtkSmartPointer<context_items::Points>::New ();
  points->setColors (static_cast<unsigned char> (r*255.0), 
                     static_cast<unsigned char> (g*255.0), 
                     static_cast<unsigned char> (b*255.0));
  points->setOpacity (opacity);
  points->set (xy);
  am_it->actor->GetScene ()->AddItem (points);
  return (true);
}
コード例 #6
0
ファイル: image_viewer.hpp プロジェクト: TuZZiX/ROS_IDE_inc
template <typename T> bool
pcl::visualization::ImageViewer::addPlanarPolygon (
    const typename pcl::PointCloud<T>::ConstPtr &image,
    const pcl::PlanarPolygon<T> &polygon, 
    const std::string &layer_id, double opacity)
{
  // We assume that the data passed into image is organized, otherwise this doesn't make sense
  if (!image->isOrganized ())
    return (false);

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addPlanarPolygon] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, image_viewer_->GetRenderWindow ()->GetSize ()[0] - 1, image_viewer_->GetRenderWindow ()->GetSize ()[1] - 1, opacity, true);
  }

  am_it->canvas->SetDrawColor (255.0, 0.0, 0.0, opacity * 255.0);

  // Construct a search object to get the camera parameters
  pcl::search::OrganizedNeighbor<T> search;
  search.setInputCloud (image);
  for (size_t i = 0; i < polygon.getContour ().size () - 1; ++i)
  {
    pcl::PointXY p1, p2;
    search.projectPoint (polygon.getContour ()[i], p1);
    search.projectPoint (polygon.getContour ()[i+1], p2);

    am_it->canvas->DrawSegment (int (p1.x), int (float (image->height) - p1.y),
                                int (p2.x), int (float (image->height) - p2.y));
  }

  // Close the polygon
  pcl::PointXY p1, p2;
  search.projectPoint (polygon.getContour ()[polygon.getContour ().size () - 1], p1);
  search.projectPoint (polygon.getContour ()[0], p2);

  am_it->canvas->DrawSegment (int (p1.x), int (float (image->height) - p1.y),
                              int (p2.x), int (float (image->height) - p2.y));
  return (true);
}
コード例 #7
0
ファイル: image_viewer.hpp プロジェクト: gpicchiarelli/pcl
template <typename PointT> bool
pcl::visualization::ImageViewer::showCorrespondences (
  const pcl::PointCloud<PointT> &source_img,
  const pcl::PointCloud<PointT> &target_img,
  const pcl::Correspondences &correspondences,
  int nth,
  const std::string &layer_id)
{
  if (correspondences.empty ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
    return (false);
  }

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addCorrespondences] No layer with ID='%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, source_img.width + target_img.width, std::max (source_img.height, target_img.height), 1.0, false);
  }
 
  int src_size = source_img.width * source_img.height * 3;
  int tgt_size = target_img.width * target_img.height * 3;

  // Set window size
  setSize (source_img.width + target_img.width , std::max (source_img.height, target_img.height));

  // Set data size
  if (data_size_ < (src_size + tgt_size))
  {
    data_size_ = src_size + tgt_size;
    data_.reset (new unsigned char[data_size_]);
  }

  // Copy data in VTK format
  int j = 0;
  for (size_t i = 0; i < std::max (source_img.height, target_img.height); ++i)
  {
    // Still need to copy the source?
    if (i < source_img.height)
    {
      for (size_t k = 0; k < source_img.width; ++k)
      {
        data_[j++] = source_img[i * source_img.width + k].r;
        data_[j++] = source_img[i * source_img.width + k].g;
        data_[j++] = source_img[i * source_img.width + k].b;
      }
    }
    else
    {
      memcpy (&data_[j], 0, source_img.width * 3);
      j += source_img.width * 3;
    }

    // Still need to copy the target?
    if (i < source_img.height)
    {
      for (size_t k = 0; k < target_img.width; ++k)
      {
        data_[j++] = target_img[i * source_img.width + k].r;
        data_[j++] = target_img[i * source_img.width + k].g;
        data_[j++] = target_img[i * source_img.width + k].b;
      }
    }
    else
    {
      memcpy (&data_[j], 0, target_img.width * 3);
      j += target_img.width * 3;
    }
  }

  void* data = const_cast<void*> (reinterpret_cast<const void*> (data_.get ()));
  
  vtkSmartPointer<vtkImageData> image = vtkSmartPointer<vtkImageData>::New ();
  image->SetDimensions (source_img.width + target_img.width, std::max (source_img.height, target_img.height), 1);
  image->SetScalarTypeToUnsignedChar ();
  image->SetNumberOfScalarComponents (3);
  image->AllocateScalars ();
  image->GetPointData ()->GetScalars ()->SetVoidArray (data, data_size_, 1);
  vtkSmartPointer<PCLContextImageItem> image_item = vtkSmartPointer<PCLContextImageItem>::New ();
#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION < 10))
  // Now create filter and set previously created transformation
  algo_->SetInput (image);
  algo_->Update ();
  image_item->set (0, 0, algo_->GetOutput ());
#else
  image_item->set (0, 0, image);
  interactor_style_->adjustCamera (image, ren_);
#endif
  am_it->actor->GetScene ()->AddItem (image_item);
  image_viewer_->SetSize (image->GetDimensions ()[0], image->GetDimensions ()[1]);

  // Draw lines between the best corresponding points
  for (size_t i = 0; i < correspondences.size (); i += nth)
  {
    double r, g, b;
    getRandomColors (r, g, b);
    unsigned char u_r = static_cast<unsigned char> (255.0 * r);
    unsigned char u_g = static_cast<unsigned char> (255.0 * g);
    unsigned char u_b = static_cast<unsigned char> (255.0 * b);
    vtkSmartPointer<context_items::Circle> query_circle = vtkSmartPointer<context_items::Circle>::New ();
    query_circle->setColors (u_r, u_g, u_b);
    vtkSmartPointer<context_items::Circle> match_circle = vtkSmartPointer<context_items::Circle>::New ();
    match_circle->setColors (u_r, u_g, u_b);
    vtkSmartPointer<context_items::Line> line = vtkSmartPointer<context_items::Line>::New ();
    line->setColors (u_r, u_g, u_b);

    float query_x = correspondences[i].index_query % source_img.width;
    float match_x = correspondences[i].index_match % target_img.width + source_img.width;
#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION >= 10))
    float query_y = correspondences[i].index_query / source_img.width;
    float match_y = correspondences[i].index_match / target_img.width;
#else
    float query_y = getSize ()[1] - correspondences[i].index_query / source_img.width;
    float match_y = getSize ()[1] - correspondences[i].index_match / target_img.width;
#endif

    query_circle->set (query_x, query_y, 3.0);
    match_circle->set (match_x, match_y, 3.0);
    line->set (query_x, query_y, match_x, match_y);

    am_it->actor->GetScene ()->AddItem (query_circle);
    am_it->actor->GetScene ()->AddItem (match_circle);
    am_it->actor->GetScene ()->AddItem (line);
  }
  
  return (true);
}
コード例 #8
0
ファイル: image_viewer.hpp プロジェクト: BITVoyager/pcl
template <typename T> bool
pcl::visualization::ImageViewer::addRectangle (
    const typename pcl::PointCloud<T>::ConstPtr &image,
    const T &min_pt,
    const T &max_pt,
    double r, double g, double b,
    const std::string &layer_id, double opacity)
{
  // We assume that the data passed into image is organized, otherwise this doesn't make sense
  if (!image->isOrganized ())
    return (false);

  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
  if (am_it == layer_map_.end ())
  {
    PCL_DEBUG ("[pcl::visualization::ImageViewer::addRectangle] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
    am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, false);
  }

  // Construct a search object to get the camera parameters
  pcl::search::OrganizedNeighbor<T> search;
  search.setInputCloud (image);
  // Project the 8 corners
  T p1, p2, p3, p4, p5, p6, p7, p8;
  p1.x = min_pt.x; p1.y = min_pt.y; p1.z = min_pt.z;
  p2.x = min_pt.x; p2.y = min_pt.y; p2.z = max_pt.z;
  p3.x = min_pt.x; p3.y = max_pt.y; p3.z = min_pt.z;
  p4.x = min_pt.x; p4.y = max_pt.y; p4.z = max_pt.z;
  p5.x = max_pt.x; p5.y = min_pt.y; p5.z = min_pt.z;
  p6.x = max_pt.x; p6.y = min_pt.y; p6.z = max_pt.z;
  p7.x = max_pt.x; p7.y = max_pt.y; p7.z = min_pt.z;
  p8.x = max_pt.x; p8.y = max_pt.y; p8.z = max_pt.z;

  std::vector<pcl::PointXY> pp_2d (8);
  search.projectPoint (p1, pp_2d[0]);
  search.projectPoint (p2, pp_2d[1]);
  search.projectPoint (p3, pp_2d[2]);
  search.projectPoint (p4, pp_2d[3]);
  search.projectPoint (p5, pp_2d[4]);
  search.projectPoint (p6, pp_2d[5]);
  search.projectPoint (p7, pp_2d[6]);
  search.projectPoint (p8, pp_2d[7]);

  pcl::PointXY min_pt_2d, max_pt_2d;
  min_pt_2d.x = min_pt_2d.y = std::numeric_limits<float>::max ();
  max_pt_2d.x = max_pt_2d.y = -std::numeric_limits<float>::max ();
  // Search for the two extrema
  for (size_t i = 0; i < pp_2d.size (); ++i)
  {
    if (pp_2d[i].x < min_pt_2d.x) min_pt_2d.x = pp_2d[i].x;
    if (pp_2d[i].y < min_pt_2d.y) min_pt_2d.y = pp_2d[i].y;
    if (pp_2d[i].x > max_pt_2d.x) max_pt_2d.x = pp_2d[i].x;
    if (pp_2d[i].y > max_pt_2d.y) max_pt_2d.y = pp_2d[i].y;
  }
  min_pt_2d.y = float (image->height) - min_pt_2d.y;
  max_pt_2d.y = float (image->height) - max_pt_2d.y;

  vtkSmartPointer<context_items::Rectangle> rect = vtkSmartPointer<context_items::Rectangle>::New ();
  rect->setColors (static_cast<unsigned char> (255.0 * r), 
                   static_cast<unsigned char> (255.0 * g), 
                   static_cast<unsigned char> (255.0 * b));
  rect->setOpacity (opacity);
  rect->set (min_pt_2d.x, min_pt_2d.y, max_pt_2d.x, max_pt_2d.y);
  am_it->actor->GetScene ()->AddItem (rect);

  return (true);
}