add support for multiple objects

testing/fakeTriangle.py

@@ -56,7 +56,7 @@ def __init__(self, pMark=None, sMark=None, imSize=None, gamma=None):
       self.sMark = sMark
       self.noMarker = False
     else:
-      warnings('fakeTriangle: pMark and sMark are incompatible sizes.')
+      warnings.warn('fakeTriangle: pMark and sMark are incompatible sizes.')
 
     self.imSize = imSize
 

testing/trackTri02.py

@@ -0,0 +1,146 @@
+#=============================== trackTri02 ==============================
+#
+# @brief    Code to create an image sequence of multiple objects moving along a
+#           twist trajectory (constant body velocity) with the presumed
+#           marker measurements that would result. Marker can be a filled
+#           area.
+#
+#
+# This script helps to understand how to generate fake marker imagery to
+# test out codebase for processing marker movement. There are two options
+# provided, one is with a moving set of three markers in a triangle shape.
+# They move rigidly together.  The other is no markers and the triangle is
+# a solid shape. It is developed based on fakeTri02.py.
+#
+#=============================== trackTri02 ==============================
+
+#
+# @file     trackTri02.py
+#
+# @author   Yunzhi Lin,         [email protected]
+#
+# @date     2021/07/21 [created]
+#
+#!NOTE:
+#!  Indent is set to 2 spaces.
+#!  Tab is set to 4 spaces with conversion to spaces.
+#
+# @quit
+#=============================== trackTri02 ==============================
+
+
+#==[0] Prep environment.
+#
+import operator
+import numpy as np
+import matplotlib.pyplot as plt
+
+from fakeTriangle import fakeTriangle
+import Lie.group.SE2.Homog
+import improcessor.basic as improcessor
+import detector.inImage as detector
+import trackpointer.centroidMulti as tracker
+
+#==[1] Specify the marker geometry on the rigid body, and other related
+#       parameters. Instantiate the simulated image generator.
+#
+# Currently, x,y follow the OpenCV coordinate system but not the Matlab one
+pMark_list=[]
+pMark_1  = np.array([[-12, -12, 12],[18, -18, 0],[1, 1, 1]])+np.array([[120],[120],[0]]) # Define a triangle
+pMark_2  = np.array([[-12, -12, 12],[18, -18, 0],[1, 1, 1]])+np.array([[60],[60],[0]]) # Define a triangle
+
+pMark_list.append(pMark_1)
+pMark_list.append(pMark_2)
+
+sMark  = 2*np.array([[ -2, -2, 2, 2],[-2, 2, 2, -2],[0, 0, 0, 0]]) # For each vertice
+imSize = np.array([200, 200])
+
+# @todo comment this line to enable useMarkers or not
+useMarkers = False
+# useMarkers = True
+
+ftarg_list=[]
+if useMarkers:
+  for pMark in pMark_list:
+    ftarg_list.append(fakeTriangle(pMark, sMark, imSize))
+else:
+  for pMark in pMark_list:
+    ftarg_list.append(fakeTriangle(pMark, None, imSize))
+
+
+#==[2] Define the marker tracking interface. Consists of a color detector (binary) and a
+#       track pointer.
+#
+
+#----[2.1] Target detector.
+#
+
+improc = improcessor.basic(operator.gt,(0,),
+                           improcessor.basic.to_uint8,())
+binDet = detector.inImage(improc)
+
+#----[2.2] Target tracker. For now a centroid tracker.
+#
+trackptr = tracker.centroidMulti()
+
+
+#==[3] Define initial condition, set the pose and the relative
+#       transformation, then render the image sequence in a for loop.
+#
+
+theta = 0
+R = Lie.group.SE2.Homog.rotationMatrix(theta)
+x = np.array([[200],[200]])
+g = Lie.group.SE2.Homog(R=R, x=x)
+
+for ftarg in ftarg_list:
+  ftarg.setPose(g)
+
+plt.ion()
+
+# Render first frame @ initial pose.
+I = np.zeros(imSize)
+for ftarg in ftarg_list:
+  I = (I.astype('bool') | ftarg.render().astype('bool')).astype('uint8')
+
+plt.imshow(I, cmap='Greys')
+
+for ii in range(1000):# Loop to udpate pose and re-render.
+
+  theta = np.pi/200
+  R = Lie.group.SE2.Homog.rotationMatrix(theta)
+  g = g * Lie.group.SE2.Homog(x=np.array([[0],[0]]), R=R)
+  for ftarg in ftarg_list:
+    ftarg.setPose(g)
+
+  I = np.zeros(imSize)
+  for ftarg in ftarg_list:
+    I = (I.astype('bool') | ftarg.render().astype('bool')).astype('uint8')
+
+  binDet.process(I)
+
+  dI = binDet.Ip
+
+  trackptr.process(dI)
+  tstate = trackptr.getstate()
+
+  # @todo
+  # There is no setting in the trackTri01.m to enable the display of the tracker as it uses
+  # triangleSE2 instead of centroid, which are slightly different in their displayState functions.
+  # For now, we manully set the state.
+  if ii==0:
+    # Start tracking
+    trackptr.setstate(tstate.tpt)
+
+  plt.cla()
+  trackptr.displayState(tstate)
+
+  plt.imshow(I, cmap='Greys')
+  plt.pause(0.001)
+
+plt.ioff()
+plt.draw()
+
+
+#
+#=============================== trackTri02 ==============================

trackpointer/centroidMulti.py

@@ -0,0 +1,184 @@
+#================================ centroidMulti ===============================
+#
+# @brief    Used to track the centroid of multiple (binarized) objects.
+#
+# Performs tracking of multiple objects by computing their centroids.
+#
+# There are two ways to perform centroid tracking.  One is to presume
+# that the object to track has already been binarized and it is simply a
+# matter of computing the centroid.  The other is that the binarization
+# needs to first be computed, then the centroid calculated.  If
+# binarization of the input is desired, use an ``improcessor`` to perform the
+# binarization as a pre-processing step.  If the binarization is too
+# complicated for an improcess procedure, then write a tracker wrapper around
+# the binarization + tracking combination. It will usually then become a
+# ``perceiver`` object.
+#
+#================================ centroidMulti ===============================
+
+#
+# @file     centroidMulti.m
+#
+# @author   Patricio A. Vela,       [email protected]
+#           Yunzhi Lin,             [email protected]
+# @date     2020/11/10  [created]
+#           2021/07/21  [modified]
+#
+#!NOTE:
+#!  set indent to 2 spaces.
+#!  do not indent function code.
+#!  set tab to 4 spaces with conversion to spaces.
+#
+#
+#================================ centroid ===============================
+
+import numpy as np
+import matplotlib.pyplot as plt
+import cv2
+
+from skimage.measure import regionprops
+from trackpointer.centroid import centroid
+
+class State(object):
+
+  def __init__(self, tpt=None, haveMeas=None):
+    self.tpt = tpt
+    self.haveMeas = haveMeas
+
+class Params(object):
+
+  def __init__(self):
+    pass
+
+class centroidMulti(centroid):
+
+  # ============================== centroid =============================
+  #
+  # @brief      Centroid track-pointer constructor.
+  #
+  # @param[in]  iPt     The initial track point coordinates.
+  # @param[in]  params   The parameter structure.
+  #
+  def __init__(self, iPt=None, params=None):
+
+    super(centroidMulti,self).__init__(iPt,params)
+
+
+  #=============================== set ===============================
+  #
+  # @brief  Set parameters for the tracker.
+  #
+  # @param[in]  fname       Name of parameter.
+  # @param[in]  fval        Value of parameter.
+  #
+  def set(self, fname, fval):
+
+    # @todo    fill out.
+
+    pass
+
+
+  #=============================== get ===============================
+  #
+  # @brief  Get parameter of the tracker.
+  #
+  # @param[in]  fname       Name of parameter.
+  # @param[out] fval        Value of parameter.
+  #
+  def get(self, fname):
+
+    # @todo    fill out.
+
+    pass
+
+  #============================== measure ==============================
+  #
+  # @brief  Measure the track point from the given image.
+  #
+  # @param[in]  I   The input image.
+  #
+  def measure(self, I):
+
+    if hasattr(self.tparams, 'improcessor') and self.tparams.improcessor:
+      Ip = self.tparams.improcessor.apply(I)
+    else:
+      Ip = I
+    plt.imshow(Ip)
+    plt.show()
+
+    binReg = centroidMulti.regionProposal(Ip)
+    self.tpt =  np.array(binReg).T # from N x 2 to 2 x N
+
+    self.haveMeas = self.tpt.shape[1] > 0
+
+    # @todo
+    # Not sure if the translation is correct
+    # if (nargout == 1):
+    #   mstate = this.getstate();
+    # end
+    mstate = self.getstate()
+
+    return mstate
+
+  #============================== process ==============================
+  #
+  # @brief  Process the input image according to centroid tracking.
+  #
+  # @param[in]  I   The input image.
+  #
+  def process(self, I):
+
+    self.measure(I)
+
+  #============================ displayState ===========================
+  #
+  # @brief  Displays the current track pointer measurement.
+  #
+  # Assumes that the current figure to plot to is activate.  If the plot has
+  # existing elements that should remain, then hold should be enabled prior to
+  # invoking this function.
+  #
+  def displayState(self, dstate = None):
+
+    if dstate:
+      if dstate.haveMeas:
+        plt.plot(dstate.tpt[0,:], dstate.tpt[1,:], self.tparams.plotStyle)
+    else:
+      if self.haveMeas:
+        plt.plot(self.tpt[0,:], self.tpt[1,:], self.tparams.plotStyle)
+
+
+  #========================= displayDebugState =========================
+  #
+  # @brief  Displays internally stored intermediate process output.
+  #
+  # Currently, there is no intermediate output, though that might change
+  # in the future.
+  #
+  def displayDebugState(self, dbstate=None):
+    pass
+
+  #========================= regionProposal =========================
+  #
+  # @brief  Find out the centroid for multiple objects
+  #
+  # @param[in]  I   The input mask image.
+  #
+  @ staticmethod
+  def regionProposal(I):
+    mask = np.zeros_like(I)
+    cnts = cv2.findContours(I, cv2.RETR_EXTERNAL,
+                            cv2.CHAIN_APPROX_SIMPLE)
+    # For OpenCV 4+
+    cnts = cnts[0]
+
+    for idx, cnt in enumerate(cnts):
+      cv2.drawContours(mask, cnt, -1, (idx+1), 1)
+
+    # Note that regionprops assumes different areas are with different labels
+    # See https://stackoverflow.com/a/61591279/5269146
+    binReg = [[i.centroid[1], i.centroid[0]] for i in regionprops(mask)]
+
+    return binReg
+#
+#================================ centroidMulti ===============================