Note
This notebook can be downloaded here: Aruco_detection_direct_courbe_angle.ipynb
import numpy as np
import matplotlib.pyplot as plt
import cv2
import cv2.aruco as aruco
import time
import pandas as pd
%matplotlib tk
# inline, nbagg
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
img = aruco.drawMarker(aruco_dict, 2, 700)
cv2.imwrite("test_marker.jpg", img)
angle=[]
cv2.waitKey(0)
cv2.destroyAllWindows()
cap = cv2.VideoCapture(0)
size_of_marker = 0.045
while(True):
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
''' detectMarkers(...)
detectMarkers(image, dictionary[, corners[, ids[, parameters[, rejectedI
mgPoints]]]]) -> corners, ids, rejectedImgPoints
'''
size_of_marker = 0.045
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
frame = aruco.drawDetectedMarkers(frame, corners)
cv2.imshow('frame',frame)
imsize = gray.shape
dist = np.zeros((5,1))
mtx = np.array([[ 2000., 0., imsize[0]/2.],
[ 0., 2000., imsize[1]/2.],
[ 0., 0., 1.]])
rvecs,tvecs, trash = aruco.estimatePoseSingleMarkers(corners, size_of_marker, mtx, dist )
angle.append(rvecs)
angle
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()
dt = 0.02 # Temps entre deux image
Nframe = len(angle)
t = np.arange(Nframe) * dt
R = np.zeros((Nframe, 3))
for i in range(Nframe):
if angle[i] is None:
R[i,:] = np.nan
else:
R[i,:] = angle[i]
out = pd.DataFrame()
out["t"] = t
out["Rx"] = np.degrees(R[:, 0])
out["Ry"] = np.degrees(R[:, 1])
out["Rz"] = np.degrees(R[:, 2])
out.index.name = "frame"
out
t | Rx | Ry | Rz | |
---|---|---|---|---|
frame | ||||
0 | 0.00 | NaN | NaN | NaN |
1 | 0.02 | NaN | NaN | NaN |
2 | 0.04 | NaN | NaN | NaN |
3 | 0.06 | NaN | NaN | NaN |
4 | 0.08 | NaN | NaN | NaN |
5 | 0.10 | NaN | NaN | NaN |
6 | 0.12 | NaN | NaN | NaN |
7 | 0.14 | NaN | NaN | NaN |
8 | 0.16 | NaN | NaN | NaN |
9 | 0.18 | NaN | NaN | NaN |
10 | 0.20 | NaN | NaN | NaN |
11 | 0.22 | NaN | NaN | NaN |
12 | 0.24 | NaN | NaN | NaN |
13 | 0.26 | NaN | NaN | NaN |
14 | 0.28 | NaN | NaN | NaN |
15 | 0.30 | NaN | NaN | NaN |
16 | 0.32 | NaN | NaN | NaN |
17 | 0.34 | NaN | NaN | NaN |
18 | 0.36 | NaN | NaN | NaN |
19 | 0.38 | NaN | NaN | NaN |
20 | 0.40 | NaN | NaN | NaN |
21 | 0.42 | NaN | NaN | NaN |
22 | 0.44 | NaN | NaN | NaN |
23 | 0.46 | NaN | NaN | NaN |
24 | 0.48 | NaN | NaN | NaN |
25 | 0.50 | NaN | NaN | NaN |
26 | 0.52 | NaN | NaN | NaN |
27 | 0.54 | NaN | NaN | NaN |
28 | 0.56 | NaN | NaN | NaN |
29 | 0.58 | NaN | NaN | NaN |
... | ... | ... | ... | ... |
548 | 10.96 | 112.616365 | 123.675515 | -32.376848 |
549 | 10.98 | 110.689358 | 125.841417 | -32.226566 |
550 | 11.00 | 109.985899 | 126.922315 | -29.990246 |
551 | 11.02 | 112.962763 | 129.407285 | -28.748873 |
552 | 11.04 | 111.537820 | 127.950517 | -33.088528 |
553 | 11.06 | 108.779533 | 127.127758 | -32.424396 |
554 | 11.08 | 108.748064 | 127.543609 | -30.340518 |
555 | 11.10 | 106.893078 | 126.890596 | -30.733698 |
556 | 11.12 | 107.203824 | 128.077016 | -31.727598 |
557 | 11.14 | 106.673280 | 128.877379 | -34.661211 |
558 | 11.16 | 106.045870 | 129.255417 | -34.820648 |
559 | 11.18 | 106.156150 | 130.859854 | -32.891079 |
560 | 11.20 | 107.553999 | 131.699757 | -31.837569 |
561 | 11.22 | 107.274529 | 130.818850 | -32.168542 |
562 | 11.24 | 106.388770 | 129.698420 | -34.417733 |
563 | 11.26 | 105.204035 | 128.819671 | -35.438550 |
564 | 11.28 | 106.147819 | 129.701578 | -34.835199 |
565 | 11.30 | 103.868823 | 127.923984 | -34.372065 |
566 | 11.32 | 104.221034 | 128.070224 | -33.716344 |
567 | 11.34 | 104.919628 | 128.773843 | -34.580152 |
568 | 11.36 | 105.228644 | 128.442850 | -34.669834 |
569 | 11.38 | 104.998862 | 127.552461 | -35.476936 |
570 | 11.40 | 104.633700 | 127.194306 | -35.719187 |
571 | 11.42 | 102.687253 | 126.837849 | -34.200362 |
572 | 11.44 | 102.172307 | 126.291363 | -34.012023 |
573 | 11.46 | 101.908830 | 127.839283 | -34.460761 |
574 | 11.48 | 103.063066 | 129.827542 | -33.316156 |
575 | 11.50 | 105.050463 | 132.241574 | -30.550074 |
576 | 11.52 | 103.379668 | 128.878511 | -30.982259 |
577 | 11.54 | 103.308638 | 128.442769 | -32.294583 |
578 rows × 4 columns
plt.figure()
plt.plot(out.t, abs(out.Rz))
plt.show()
Rz = [a for a in Rz if a is not None]
Rz
---------------------------------------------------------------------------
NameError Traceback (most recent call last)
<ipython-input-22-e027601dcd54> in <module>()
----> 1 Rz = [a for a in Rz if a is not None]
2 Rz
NameError: name 'Rz' is not defined
plt.figure()
x = points[:, 1]
plt.plot(x,angle, "oscillations")
plt.show()
---------------------------------------------------------------------------
ValueError Traceback (most recent call last)
<ipython-input-16-9a9b9b3d9e9e> in <module>()
1 plt.figure()
----> 2 plt.plot(np.random.rand(10),angle, "or-")
3 plt.show()
R:\Anaconda3\lib\site-packages\matplotlib\pyplot.py in plot(*args, **kwargs)
3259 mplDeprecation)
3260 try:
-> 3261 ret = ax.plot(*args, **kwargs)
3262 finally:
3263 ax._hold = washold
R:\Anaconda3\lib\site-packages\matplotlib\__init__.py in inner(ax, *args, **kwargs)
1715 warnings.warn(msg % (label_namer, func.__name__),
1716 RuntimeWarning, stacklevel=2)
-> 1717 return func(ax, *args, **kwargs)
1718 pre_doc = inner.__doc__
1719 if pre_doc is None:
R:\Anaconda3\lib\site-packages\matplotlib\axes\_axes.py in plot(self, *args, **kwargs)
1370 kwargs = cbook.normalize_kwargs(kwargs, _alias_map)
1371
-> 1372 for line in self._get_lines(*args, **kwargs):
1373 self.add_line(line)
1374 lines.append(line)
R:\Anaconda3\lib\site-packages\matplotlib\axes\_base.py in _grab_next_args(self, *args, **kwargs)
402 this += args[0],
403 args = args[1:]
--> 404 for seg in self._plot_args(this, kwargs):
405 yield seg
406
R:\Anaconda3\lib\site-packages\matplotlib\axes\_base.py in _plot_args(self, tup, kwargs)
382 x, y = index_of(tup[-1])
383
--> 384 x, y = self._xy_from_xy(x, y)
385
386 if self.command == 'plot':
R:\Anaconda3\lib\site-packages\matplotlib\axes\_base.py in _xy_from_xy(self, x, y)
241 if x.shape[0] != y.shape[0]:
242 raise ValueError("x and y must have same first dimension, but "
--> 243 "have shapes {} and {}".format(x.shape, y.shape))
244 if x.ndim > 2 or y.ndim > 2:
245 raise ValueError("x and y can be no greater than 2-D, but have "
ValueError: x and y must have same first dimension, but have shapes (10,) and (958,)
Help on built-in function detectMarkers: detectMarkers(...) detectMarkers(image, dictionary[, corners[, ids[, parameters[, rejectedImgPoints[, cameraMatrix[, distCoeff]]]]]]) -> corners, ids, rejectedImgPoints . * @brief Basic marker detection . * . * @param image input image . * @param dictionary indicates the type of markers that will be searched . * @param corners vector of detected marker corners. For each marker, its four corners . * are provided, (e.g std::vector<std::vector<cv::Point2f> > ). For N detected markers, . * the dimensions of this array is Nx4. The order of the corners is clockwise. . * @param ids vector of identifiers of the detected markers. The identifier is of type int . * (e.g. std::vector<int>). For N detected markers, the size of ids is also N. . * The identifiers have the same order than the markers in the imgPoints array. . * @param parameters marker detection parameters . * @param rejectedImgPoints contains the imgPoints of those squares whose inner code has not a . * correct codification. Useful for debugging purposes. . * @param cameraMatrix optional input 3x3 floating-point camera matrix . * f$A = vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}f$ . * @param distCoeff optional vector of distortion coefficients . * f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])f$ of 4, 5, 8 or 12 elements . * . * Performs marker detection in the input image. Only markers included in the specific dictionary . * are searched. For each detected marker, it returns the 2D position of its corner in the image . * and its corresponding identifier. . * Note that this function does not perform pose estimation. . * @sa estimatePoseSingleMarkers, estimatePoseBoard . *