1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
|
/**
\page tutorial-pose-estimation Tutorial: Pose estimation from points
\tableofcontents
\section tutorial_pose_intro Introduction
This tutorial focuses on pose estimation from planar or non planar points. From their 2D coordinates in the image plane, and their corresponding 3D coordinates specified in an object frame, ViSP is able to estimate the relative pose between the camera and the object frame. This pose is returned as an homogeneous matrix cMo. Note that to estimate a pose at least four points need to be considered.
In this tutorial we assume that you are familiar with ViSP blob tracking. If not, see \ref tutorial-tracking-blob.
Note that all the material (source code and image) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
\code
$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/computer-vision
\endcode
\section tutorial_pose_est Pose estimation from 4 planar points
\subsection tutorial_pose_est_image Principle on a single image
In this section we consider the case of four planar points. An image processing done by our blob tracker allows to extract the pixel coordinates of each blob center of gravity. Then using the camera intrinsic parameters we convert these pixel coordinates in meters in the image plane. Knowing the 3D coordinates of the points we then estimate the pose of the object.
The corresponding source code also provided in tutorial-pose-from-points-image.cpp is the following.
\include tutorial-pose-from-points-image.cpp
Here is a screen shot of the resulting program:
\image html img-pose-square.jpg
And here is the detailed explanation of some lines introduced in the source code.
First we include the header pose_helper.h that contains helper functions dedicated to pose estimation.
\snippet tutorial-pose-from-points-image.cpp Include
In pose_helper.h header we include the vpPoint header class that is a container for 3D points with their 3D coordinates in the object frame, and their 2D coordinates in the image plane obtained after perspective projection. These 3D and 2D coordinates will be used as input in the pose estimation algorithm implemented in vpPose class. Then we include the vpCameraParameters header class that contains the camera intrinsic parameters. These parameters are typically used to convert coordinates of a blog cog obtained in pixels from the tracking of the blob, into 2D coordinates expressed in the image plane. The next header concerns the vpHomogeneousMatrix class that is a pose container. Finally, the vpDot2 header class corresponds to the tracker that will be used to track the 4 blobs.
\snippet pose_helper.h Include
If we have a look in pose_helper.cpp we will find the `computePose()` function that does the pose estimation. This function uses as input two vectors. The first one is a vector of vpPoint that contains the 3D coordinates in meters of a point, while the second one is a vector of vpImagePoints that contains the 2D coordinates in pixels of a blob center of gravity. The 3D and 2D coordinates of the points need to be matched. That means that `point[i]` and `ip[i]` should refer to the same physical point. Other inputs are \c cam that corresponds to the camera intrinsic parameters, and \c init that indicates if the pose needs to be initialized the first time. We have \c cMo [in/out] parameter that is as input the pose estimated from the previous image, and, as output the resulting pose estimated from the input points.
\snippet pose_helper.cpp Compute pose
In the `computePose()` function, we create first an instance of the vpPose class. Then we convert each coordinates of the blob center
of gravity `ip` in 2D coordinates \c (x,y) in meter in the image plane using vpPixelMeterConversion::convertPoint(). We update then the
\c point instance with the corresponding 2D coordinates and add this point to the vpPose class.
Once all the points are added, when `init` is `true` we can estimate an initial pose using vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS method that runs first Dementhon and Lagrange
linear pose estimation algorithms, then retain the result that has the lowest residual and finaly use this result to initialize the non linear virtual visual-servoing algorithm to estimate the pose
that will converge to the solution with a lower residue than on of the linear approaches.
When `init` is `false`, we consider that the previous pose `cMo` is near the solution so that it can be used as initialization for the non linear visual servoing minimization.
If we come back in tutorial-pose-from-points-image.cpp in the `main()` function, instead of using an image sequence of the object, we consider always the same image `square.pgm`. In this image we consider a 12cm by 12 cm square. The corners are represented by a blob. Their center of gravity correspond to a corner position. This gray level image is read thanks to:
\snippet tutorial-pose-from-points-image.cpp Read image
After the instantiation of a window able to display the image and at the end the result of the estimated pose as a pose frame, we setup the camera parameters. We consider here the following camera model \f${\bf a}=(p_x,p_y, u_0,v_0)\f$ where \f$p_x = p_y = 800 \f$ are the ratio between the focal length and the size of a pixel, and where the principal point \f$(u_0, v_0)\f$ is located at the image center position.
\snippet tutorial-pose-from-points-image.cpp Camera parameters
Each blob is then tracked using a vector of four vpDot2 trackers. To avoid human interaction, we initialize the tracker with a pixel position inside each blob. Here we also instantiate a vector of vpImagePoint `ip` that will be updated in the while loop with the 2D coordinates in pixels of the blob center of gravity.
\snippet tutorial-pose-from-points-image.cpp Blob tracking
We also define the 3D model of our 12cm by 12cm square by setting the 3D coordinates of the corners in an object frame located in this example in the middle of the square. We consider here that the points are located in the plane \f$z=0\f$.
\snippet tutorial-pose-from-points-image.cpp 3D model
Next, we created an homogeneous matrix \c cMo that will contain the estimated pose.
\snippet tutorial-pose-from-points-image.cpp Homogeneous matrix
In the infinite loop, at each iteration we read a new image (in this example it remains the same), display its content in the window, track each blob and update the coordinates of the center of gravity in `ip` vector.
\snippet tutorial-pose-from-points-image.cpp Tracking
Then we call the pose estimation function `computePose() presented previously. It uses the 3D coordinates of the points defined as our model in the object frame, and their corresponding 2D positions in the image obtained by the blob tracker to estimate the pose \c cMo. This homogeneous transformation gives the position of the object frame in the camera frame.
\snippet tutorial-pose-from-points-image.cpp Pose estimation
The resulting pose is displayed as an RGB frame in the window overlay. Red, green and blue colors are for X, Y and Z axis respectively. Each axis is 0.05 meter long. All the drawings are then flushed to update the window content.
\snippet tutorial-pose-from-points-image.cpp Display pose
At the end of the first iteration, we turn off the \c init flag that indicates that the next pose estimation could use our non linear vpPose::VIRTUAL_VS estimation method with the previous pose as initial value.
\snippet tutorial-pose-from-points-image.cpp Next pose uses previous one as initialization
Finally, we interrupt the infinite loop by a user mouse click in the window.
\snippet tutorial-pose-from-points-image.cpp The end
We also introduce a 40 milliseconds sleep to slow down the loop and relax the CPU.
\snippet tutorial-pose-from-points-image.cpp Slow down
\subsection tutorial_pose_est_live Using a camera
We provide also an other example in tutorial-pose-from-points-live.cpp that works on live images acquired by a camera. Compared to the previous example, we replace the single image reader by one of the frame grabber available in ViSP (see \ref tutorial-grabber).
<b>1. Download and print, a square made of 4 blobs</b>
- 4 black blobs on a white background defining a 0.1 meter width square [<a href="http://visp-doc.inria.fr/download/square-blob/square-10cm-black.pdf" target="_blank">square-10cm-black.pdf</a>] (recommended);
- 4 white blobs on a black background defining a 0.1 meter width square [<a href="http://visp-doc.inria.fr/download/square-blob/square-10cm-white.pdf" target="_blank">square-10cm-white.pdf</a>].
<b>2. Run the corresponding binary</b>
To get the usage of the binary run:
\code
./tutorial-pose-from-points-live --help
\endcode
Have a trial adapting the size of your square:
\code
./tutorial-pose-from-points-live --square_width 0.1
\endcode
If your camera is calibrated (see \ref tutorial-calibration-intrinsic), you may use your own camera parameters with something similar to:
\code
./tutorial-pose-from-points-live --square_width 0.1 --intrinsic camera.xml --camera_name Camera
\endcode
If you get the following error:
\code
$ ./tutorial-pose-from-points-live --square_width 0.1
Catch an exception: No cameras found
\endcode
it means probably that your camera is not detected by the first grabber that is enabled using:
\snippet tutorial-pose-from-points-live.cpp Grabber
If this is the case we suggest to modify the following lines in tutorial-pose-from-points-live.cpp by uncommenting all the grabber that you don't want to use:
\snippet tutorial-pose-from-points-live.cpp Undef grabber
For example, if you want to force OpenCV usage to grab images, uncomment the lines:
\code
#undef VISP_HAVE_V4L2
#undef VISP_HAVE_DC1394
#undef VISP_HAVE_CMU1394
#undef VISP_HAVE_FLYCAPTURE
#undef VISP_HAVE_REALSENSE2
\endcode
After modification, build and run again this binary.
\section tutorial_pose_next Next tutorial
You are now ready to see the next \ref tutorial-pose-estimation-qrcode that gives a use case.
If you have an RGB-D camera, you may also continue with \ref tutorial-planar-object-pose.
*/
|
