← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/doc/py_tutorials/py_calib3d/py_pose/py_pose.markdown

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sw/ext/opencv_bebop/opencv/doc/py_tutorials/py_calib3d/py_pose/py_pose.markdown
 
1
Pose Estimation {#tutorial_py_pose}
 
2
===============
 
3
 
 
4
Goal
 
5
----
 
6
 
 
7
In this section,
 
8
    -   We will learn to exploit calib3d module to create some 3D effects in images.
 
9
 
 
10
Basics
 
11
------
 
12
 
 
13
This is going to be a small section. During the last session on camera calibration, you have found
 
14
the camera matrix, distortion coefficients etc. Given a pattern image, we can utilize the above
 
15
information to calculate its pose, or how the object is situated in space, like how it is rotated,
 
16
how it is displaced etc. For a planar object, we can assume Z=0, such that, the problem now becomes
 
17
how camera is placed in space to see our pattern image. So, if we know how the object lies in the
 
18
space, we can draw some 2D diagrams in it to simulate the 3D effect. Let's see how to do it.
 
19
 
 
20
Our problem is, we want to draw our 3D coordinate axis (X, Y, Z axes) on our chessboard's first
 
21
corner. X axis in blue color, Y axis in green color and Z axis in red color. So in-effect, Z axis
 
22
should feel like it is perpendicular to our chessboard plane.
 
23
 
 
24
First, let's load the camera matrix and distortion coefficients from the previous calibration
 
25
result.
 
26
@code{.py}
 
27
import cv2
 
28
import numpy as np
 
29
import glob
 
30
 
 
31
# Load previously saved data
 
32
with np.load('B.npz') as X:
 
33
    mtx, dist, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]
 
34
@endcode
 
35
Now let's create a function, draw which takes the corners in the chessboard (obtained using
 
36
**cv2.findChessboardCorners()**) and **axis points** to draw a 3D axis.
 
37
@code{.py}
 
38
def draw(img, corners, imgpts):
 
39
    corner = tuple(corners[0].ravel())
 
40
    img = cv2.line(img, corner, tuple(imgpts[0].ravel()), (255,0,0), 5)
 
41
    img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0,255,0), 5)
 
42
    img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (0,0,255), 5)
 
43
    return img
 
44
@endcode
 
45
Then as in previous case, we create termination criteria, object points (3D points of corners in
 
46
chessboard) and axis points. Axis points are points in 3D space for drawing the axis. We draw axis
 
47
of length 3 (units will be in terms of chess square size since we calibrated based on that size). So
 
48
our X axis is drawn from (0,0,0) to (3,0,0), so for Y axis. For Z axis, it is drawn from (0,0,0) to
 
49
(0,0,-3). Negative denotes it is drawn towards the camera.
 
50
@code{.py}
 
51
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
 
52
objp = np.zeros((6*7,3), np.float32)
 
53
objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)
 
54
 
 
55
axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
 
56
@endcode
 
57
Now, as usual, we load each image. Search for 7x6 grid. If found, we refine it with subcorner
 
58
pixels. Then to calculate the rotation and translation, we use the function,
 
59
**cv2.solvePnPRansac()**. Once we those transformation matrices, we use them to project our **axis
 
60
points** to the image plane. In simple words, we find the points on image plane corresponding to
 
61
each of (3,0,0),(0,3,0),(0,0,3) in 3D space. Once we get them, we draw lines from the first corner
 
62
to each of these points using our draw() function. Done !!!
 
63
@code{.py}
 
64
for fname in glob.glob('left*.jpg'):
 
65
    img = cv2.imread(fname)
 
66
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
 
67
    ret, corners = cv2.findChessboardCorners(gray, (7,6),None)
 
68
 
 
69
    if ret == True:
 
70
        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
 
71
 
 
72
        # Find the rotation and translation vectors.
 
73
        rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist)
 
74
 
 
75
        # project 3D points to image plane
 
76
        imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
 
77
 
 
78
        img = draw(img,corners2,imgpts)
 
79
        cv2.imshow('img',img)
 
80
        k = cv2.waitKey(0) & 0xff
 
81
        if k == 's':
 
82
            cv2.imwrite(fname[:6]+'.png', img)
 
83
 
 
84
cv2.destroyAllWindows()
 
85
@endcode
 
86
See some results below. Notice that each axis is 3 squares long.:
 
87
 
 
88
![image](images/pose_1.jpg)
 
89
 
 
90
### Render a Cube
 
91
 
 
92
If you want to draw a cube, modify the draw() function and axis points as follows.
 
93
 
 
94
Modified draw() function:
 
95
@code{.py}
 
96
def draw(img, corners, imgpts):
 
97
    imgpts = np.int32(imgpts).reshape(-1,2)
 
98
 
 
99
    # draw ground floor in green
 
100
    img = cv2.drawContours(img, [imgpts[:4]],-1,(0,255,0),-3)
 
101
 
 
102
    # draw pillars in blue color
 
103
    for i,j in zip(range(4),range(4,8)):
 
104
        img = cv2.line(img, tuple(imgpts[i]), tuple(imgpts[j]),(255),3)
 
105
 
 
106
    # draw top layer in red color
 
107
    img = cv2.drawContours(img, [imgpts[4:]],-1,(0,0,255),3)
 
108
 
 
109
    return img
 
110
@endcode
 
111
Modified axis points. They are the 8 corners of a cube in 3D space:
 
112
@code{.py}
 
113
axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0],
 
114
                   [0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ])
 
115
@endcode
 
116
And look at the result below:
 
117
 
 
118
![image](images/pose_2.jpg)
 
119
 
 
120
If you are interested in graphics, augmented reality etc, you can use OpenGL to render more
 
121
complicated figures.
 
122
 
 
123
Additional Resources
 
124
--------------------
 
125
 
 
126
Exercises
 
127
---------