From b4dea4ef4742a9f8fba30124f7559029351b0020 Mon Sep 17 00:00:00 2001
From: Pavel Rojtberg <pavel.rojtberg@igd.fraunhofer.de>
Date: Fri, 7 Dec 2018 13:50:41 +0100
Subject: [PATCH] linemod: add drawFeatures method

---
 modules/rgbd/include/opencv2/rgbd/linemod.hpp |  9 ++++++++
 modules/rgbd/src/linemod.cpp                  | 23 +++++++++++++++++++
 2 files changed, 32 insertions(+)

diff --git a/modules/rgbd/include/opencv2/rgbd/linemod.hpp b/modules/rgbd/include/opencv2/rgbd/linemod.hpp
index c3ace1ea8..dc42f0634 100644
--- a/modules/rgbd/include/opencv2/rgbd/linemod.hpp
+++ b/modules/rgbd/include/opencv2/rgbd/linemod.hpp
@@ -239,6 +239,15 @@ protected:
  */
 CV_EXPORTS_W void colormap(const Mat& quantized, CV_OUT Mat& dst);
 
+/**
+ * \brief Debug function to draw linemod features
+ * @param img
+ * @param templates see @ref Detector::addTemplate
+ * @param tl template bbox top-left offset see @ref Detector::addTemplate
+ * @param size marker size see @ref cv::drawMarker
+ */
+CV_EXPORTS_W void drawFeatures(InputOutputArray img, const std::vector<Template>& templates, const Point2i& tl, int size = 10);
+
 /**
  * \brief Represents a successful template match.
  */
diff --git a/modules/rgbd/src/linemod.cpp b/modules/rgbd/src/linemod.cpp
index 7ddcd9594..f17a19257 100644
--- a/modules/rgbd/src/linemod.cpp
+++ b/modules/rgbd/src/linemod.cpp
@@ -207,6 +207,29 @@ void colormap(const Mat& quantized, Mat& dst)
   }
 }
 
+void drawFeatures(InputOutputArray img, const std::vector<Template>& templates, const Point2i& tl, int size)
+{
+#ifdef HAVE_OPENCV_IMGPROC
+    static Scalar colors[] = {{0, 0, 255}, {0, 255, 0}};
+    static int markers[] = {MARKER_SQUARE, MARKER_DIAMOND};
+
+    int modality = 0;
+    for(const Template& t : templates)
+    {
+        if(t.pyramid_level != 0) continue;
+
+        for(const Feature& f : t.features)
+        {
+            drawMarker(img, tl + Point(f.x, f.y), colors[int(modality != 0)], markers[int(modality != 0)], size);
+        }
+
+        modality++;
+    }
+#else
+    CV_Assert(false, "functionality needs imgproc module");
+#endif
+}
+
 /****************************************************************************************\
 *                             Color gradient modality                                    *
 \****************************************************************************************/