update takevideo.py

markers/RedTracker.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*
+# assuming python2 for ROS
+
+import cv2
+import numpy as np
+
+
+class RedZoneTracker:
+    def __init__(self,frame,initialize=1,tracker='KCF'):
+        # if init with hand:
+        if initialize:
+
+
+        self.get_tracker
+
+    def get_tracker(self,name):
+        """
+        Choose tracker from key word
+        """
+        self.boxtracker = {
+            'Boosting': cv2.TrackerBoosting_create(),
+            'MIL': cv2.TrackerMIL_create(),
+            'KCF' : cv2.TrackerKCF_create(),
+            'TLD' : cv2.TrackerTLD_create(),
+            'MedianFlow' : cv2.TrackerMedianFlow_create()
+        }.get(name, 0)        
+
+def extractROI(frame,roi):
+    return frame[int(roi[1]):int(roi[1]+roi[3]),int(roi[0]):int(roi[0]+roi[2])]
+
+def drawrect(frame,bbox,color=(0,255,0)):
+    p1 = (int(bbox[0]), int(bbox[1]))
+    p2 = (int(bbox[0] + bbox[2]), int(bbox[1] + bbox[3]))
+    cv2.rectangle(frame, p1, p2, color, 2, 1)
+
+
+
+def find_rect_of_target_color(image):
+    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV_FULL)
+    h = hsv[:, :, 0]
+    s = hsv[:, :, 1]
+    mask = np.zeros(h.shape, dtype=np.uint8)
+    mask[((h < 15) | (h > 200)) & (s > 128)] = 255
+    # Get boundary
+    _, contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+    rects = []
+
+    for contour in contours:
+        approx = cv2.convexHull(contour)
+        rect = cv2.boundingRect(approx)
+        rects.append(np.array(rect))
+    return max(rects, key=(lambda x: x[2] * x[3])) #return maximum rectangle
+
+
+def extractRed(image):
+    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV_FULL)
+    h = hsv[:, :, 0]
+    s = hsv[:, :, 1]
+    mask = np.zeros(h.shape, dtype=np.uint8)
+    mask[((h < 10) | (h > 200)) & (s > 128)] = 255
+
+    # get RED size
+    validnum = sum(mask.reshape(-1))/255
+    hei,wid = image.shape
+
+    Mmt = cv2.moments(mask)
+    if Mmt["m00"] != 0:
+        cx = Mmt['m10']/Mmt['m00']
+        cy = Mmt['m01']/Mmt['m00']
+        flag = True
+    else:
+        cx,cy = 0,0
+        flag = False
+    #print([cx,cy])
+    return mask,[cx,cy],flag,validnum

markers/trackmarker.py

@@ -22,28 +22,35 @@ def detect_marker1(frame):
 def detect_marker2(frame):# detect smallmarkers
     aruco = cv2.aruco
     dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
-    corners, ids, rejectedImgPoints = aruco.detectMarkers(frame, dictionary)
+    corners, ids, rejectedImgPoints = aruco.detectMarkers(frame, dictionary,parameters=param)
     dimage = aruco.drawDetectedMarkers(frame.copy(),corners)
-    return dimage
+    dfimage = aruco.drawDetectedMarkers(frame.copy(),rejectedImgPoints)
+
+    return dimage,dfimage
 
 
+param= cv2.aruco.DetectorParameters_create()
+param.minDistanceToBorder = 1
+
 if __name__=='__main__':
-    cap = cv2.VideoCapture(1)
+    cap = cv2.VideoCapture(0)
     args = sys.argv
     duration = 10000 # 10000 sample = 333s = 5.5m
     if len(args) > 1:
         duration = int(args[1])
     # Define the codec and create VideoWriter object
 
-    
+
     try:
         while ~cap.isOpened() and (duration > 0 ):
             ret,frame = cap.read()
             duration = duration - 1
             if ret==True:
 
-                dframe = detect_marker2(frame)
+                dframe,dfframe = detect_marker2(frame)
                 cv2.imshow('frame',dframe)
+                cv2.imshow('frame candidate',dfframe)
+
                 if cv2.waitKey(1) & 0xFF == ord('q'):
                     break
             else:

trial/Fields/takevideo.py

@@ -14,7 +14,7 @@
 
 
 if __name__=='__main__':
-    cap = cv2.VideoCapture(1)
+    cap = cv2.VideoCapture(0)
     args = sys.argv
     duration = 10000 # 10000 sample = 333s = 5.5m
     if len(args) > 1:

trial/Videos/CameraPoseSaving-Copy1.ipynb

@@ -2,7 +2,7 @@
  "cells": [
   {
    "cell_type": "code",
-   "execution_count": 29,
+   "execution_count": 1,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -18,26 +18,17 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 32,
+   "execution_count": 2,
    "metadata": {},
-   "outputs": [
-    {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "The autoreload extension is already loaded. To reload it, use:\n",
-      "  %reload_ext autoreload\n"
-     ]
-    }
-   ],
+   "outputs": [],
    "source": [
     "%load_ext autoreload\n",
     "%autoreload 2"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 33,
+   "execution_count": 3,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -47,7 +38,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 3,
+   "execution_count": 4,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -57,7 +48,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 35,
+   "execution_count": 5,
    "metadata": {},
    "outputs": [
     {
@@ -100,16 +91,16 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 58,
+   "execution_count": 6,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "[<matplotlib.lines.Line2D at 0x184055ea6a0>]"
+       "[<matplotlib.lines.Line2D at 0x2ee0f73f518>]"
       ]
      },
-     "execution_count": 58,
+     "execution_count": 6,
      "metadata": {},
      "output_type": "execute_result"
     },
@@ -161,7 +152,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 27,
+   "execution_count": 9,
    "metadata": {},
    "outputs": [
     {
@@ -171,36 +162,41 @@
       "[[ 0.33191806  0.9416979   0.05509514]\n",
       " [ 0.4681541  -0.11374046 -0.8762961 ]\n",
       " [-0.8189396   0.31665152 -0.47861224]]\n",
-      "[ 2.6069732 -1.3118161  1.8902268]\n"
+      "[[ 2.6069732]\n",
+      " [-1.3118161]\n",
+      " [ 1.8902268]]\n"
      ]
     }
    ],
    "source": [
     "# ture camera poses\n",
     "Rmean,_ = cv2.Rodrigues(np.float32(mrv))\n",
-    "print(Rmean)\n",
-    "tmean = -np.dot(Rmean.T,np.float32(mtv))\n",
+    "print(Rmean.T)\n",
+    "tmean = -np.dot(Rmean.T,np.float32(mtv).reshape(3,1))\n",
     "print(tmean)"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 28,
+   "execution_count": 17,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "array([0.26589057, 0.2867266 , 3.45502812])"
+       "array([[0.26589057],\n",
+       "       [0.2867266 ],\n",
+       "       [3.455028  ]], dtype=float32)"
       ]
      },
-     "execution_count": 28,
+     "execution_count": 17,
      "metadata": {},
      "output_type": "execute_result"
     }
    ],
    "source": [
-    "mtv\n",
+    "zerop = np.float32([0,0,0]).reshape(3,1)\n",
+    "np.dot(Rmean,zerop )+ np.float32(mtv).reshape(3,1)\n",
     "# then extract red patterns and get position"
    ]
   },
@@ -305,10 +301,25 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 16,
    "metadata": {},
-   "outputs": [],
-   "source": []
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "array([[-0.8189396 ],\n",
+       "       [ 0.31665152],\n",
+       "       [-0.47861224]], dtype=float32)"
+      ]
+     },
+     "execution_count": 16,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "Rmean.T.dot(np.float32([0, 0, 1]).reshape(3,1))"
+   ]
   },
   {
    "cell_type": "code",
@@ -330,19 +341,22 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 68,
+   "execution_count": 19,
    "metadata": {},
    "outputs": [
     {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "[ 1.73055377  1.26792168 -0.68694802] [0.26589057 0.2867266  3.45502812]\n"
-     ]
+     "data": {
+      "text/plain": [
+       "0.004213414634146342"
+      ]
+     },
+     "execution_count": 19,
+     "metadata": {},
+     "output_type": "execute_result"
     }
    ],
    "source": [
-    "print(mrv,mtv)\n"
+    "3.455/820"
    ]
   }
  ],

trial/trackers_test.py

@@ -111,7 +111,7 @@ def drawrect(frame,bbox,color=(0,255,0)):
     try:
         filename = sys.argv[1]
     except:
-        filename = 'Videos/square50.avi'
+        filename = 0
 
     ##---------- Method ----------------##
     try:
@@ -211,20 +211,22 @@ def drawrect(frame,bbox,color=(0,255,0)):
 p2 = np.array(pos2).reshape(-1,2)
 p3 = np.array(pos3).reshape(-1,2)
 plt.figure(1)
-plt.plot(p1[:,0],-p1[:,1],p2[:,0],-p2[:,1],p3[:,0],-p3[:,1])
-plt.legend(['Coarse&Fine','Fine','Rect'])
+plt.plot(p1[:,0],-p1[:,1],label='Coarse&Fine')
+plt.plot(p2[:,0],-p2[:,1],label='Fine')
+plt.plot(p3[:,0],-p3[:,1],label='LargestRegion')
+plt.legend()
 
 plt.figure(2)
 plt.subplot(121)
-plt.plot(p1[:,0])
-plt.plot(p2[:,0])
-plt.plot(p3[:,0])
-plt.legend(['Coarse&Fine','Fine','Rect'])
+plt.plot(p1[:,0],label='Coarse&Fine')
+plt.plot(p2[:,0],label='Fine')
+plt.plot(p3[:,0],label='LargestRegion')
+plt.legend()
 plt.subplot(122)
-plt.plot(p1[:,1])
-plt.plot(p2[:,1])
-plt.plot(p3[:,1])
-plt.legend(['Coarse&Fine','Fine','Rect'])
+plt.plot(p1[:,1],label='Coarse&Fine')
+plt.plot(p2[:,1],label='Fine')
+plt.plot(p3[:,1],label='LargestRegion')
+plt.legend()
 plt.show()
 
 # キャプチャをリリースして、ウィンドウをすべて閉じる