Mark points

visualization/include/pcl/visualization/image_viewer.h

@@ -429,6 +429,18 @@ namespace pcl
         markPoint (size_t u, size_t v, Vector3ub fg_color, Vector3ub bg_color = red_color, double radius = 3.0,
                    const std::string &layer_id = "points", double opacity = 1.0);
 
+        /** \brief Sets the pixel at coordinates(u,v) to color while setting the neighborhood to another
+          * \param[in] uv the u/x, v/y coordinate of the pixels to be marked
+          * \param[in] fg_color the pixel color
+          * \param[in] bg_color the neighborhood color
+          * \param[in] size edge of the square surrounding each pixel
+          * \param[in] layer_id the name of the layer (default: "markers")
+          * \param[in] opacity the opacity of the layer (default: 1.0)
+          */
+        void
+        markPoints (const std::vector<int>& uv, Vector3ub fg_color, Vector3ub bg_color = red_color, double size = 3.0,
+                    const std::string &layer_id = "markers", double opacity = 1.0);
+
         /** \brief Set the window title name
           * \param[in] name the window title
           */

visualization/include/pcl/visualization/vtk/pcl_context_item.h

@@ -156,6 +156,18 @@ namespace pcl
         virtual void set (float x, float y, const std::string& _text);
         std::string text;
       };
+
+      struct PCL_EXPORTS Markers : public Points
+      {
+        vtkTypeMacro (Markers, Points);
+        static Markers *New ();
+        virtual bool Paint (vtkContext2D *painter);
+        void setSize (float _size) { size = _size; }
+        void setPointColors (unsigned char r, unsigned char g, unsigned char b);
+        void setPointColors (unsigned char rgb[3]);
+        float size;
+        unsigned char point_colors[3];
+      };
     }
   }
 }

visualization/src/image_viewer.cpp

@@ -927,6 +927,48 @@ pcl::visualization::ImageViewer::markPoint (
   am_it->actor->GetScene ()->AddItem (point);
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::visualization::ImageViewer::markPoints (
+    const std::vector<int>& uv, Vector3ub fg_color, Vector3ub bg_color, double size,
+    const std::string &layer_id, double opacity)
+{
+  if (uv.size () == 0)
+    return;
+
+  // 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::markPoint] 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);
+#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 10))
+    interactor_style_->adjustCamera (ren_);
+#endif
+  }
+
+  vtkSmartPointer<context_items::Markers> markers = vtkSmartPointer<context_items::Markers>::New ();
+  markers->setOpacity (opacity);
+  std::vector<float> points (uv.size ());
+  std::size_t nb_points = points.size () / 2;
+  for (std::size_t i = 0; i < nb_points; ++i)
+  {
+    std::size_t ii = 2*i;
+    points[ii] = static_cast<float> (uv[ii]);
+#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 10))
+    points[ii] = static_cast<float> (uv[ii+1]);
+#else
+    points[ii+1] = static_cast<float> (getSize ()[1] - uv[ii+1]);
+#endif
+  }
+
+  markers->set (points);
+  markers->setSize (size);
+  markers->setColors (bg_color[0], bg_color[1], bg_color[2]);
+  markers->setPointColors (fg_color[0], fg_color[1], fg_color[2]);
+  am_it->actor->GetScene ()->AddItem (markers);
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl::visualization::ImageViewer::render ()

visualization/src/vtk/pcl_context_item.cpp

@@ -63,6 +63,7 @@ namespace pcl
       vtkStandardNewMacro (Points);
       vtkStandardNewMacro (Polygon);
       vtkStandardNewMacro (Text);
+      vtkStandardNewMacro (Markers);
     }
   }
 }
@@ -233,6 +234,37 @@ pcl::visualization::context_items::Text::Paint (vtkContext2D *painter)
   return (true);
 }
 
+///////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::visualization::context_items::Markers::setPointColors (unsigned char r, unsigned char g, unsigned char b)
+{
+  point_colors[0] = r; point_colors[1] = g; point_colors[2] = b;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::visualization::context_items::Markers::setPointColors (unsigned char rgb[3])
+{
+  memcpy (point_colors, rgb, 3 * sizeof (unsigned char));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::visualization::context_items::Markers::Paint (vtkContext2D *painter)
+{
+  int nb_points (params.size () / 2);
+  if (size <= 0)
+    size = 2.3 * painter->GetPen ()->GetWidth ();
+
+  painter->GetPen ()->SetWidth (size);
+  painter->GetPen ()->SetColor (colors);
+  painter->DrawPointSprites (0, &params[0], nb_points);
+  painter->GetPen ()->SetWidth (1);
+  painter->GetPen ()->SetColor (point_colors);
+  painter->DrawPointSprites (0, &params[0], nb_points);
+  return (true);
+}
+
 ///////////////////////////////////////////////////////////////////////////////////////////
 pcl::visualization::PCLContextImageItem::PCLContextImageItem ()
 {