cvdrawingutils.cpp
Go to the documentation of this file.
1 /*****************************
2 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without modification, are
5 permitted provided that the following conditions are met:
6 
7  1. Redistributions of source code must retain the above copyright notice, this list of
8  conditions and the following disclaimer.
9 
10  2. Redistributions in binary form must reproduce the above copyright notice, this list
11  of conditions and the following disclaimer in the documentation and/or other materials
12  provided with the distribution.
13 
14 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
15 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
16 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
17 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
18 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
19 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
20 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
21 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
22 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 
24 The views and conclusions contained in the software and documentation are those of the
25 authors and should not be interpreted as representing official policies, either expressed
26 or implied, of Rafael Muñoz Salinas.
27 ********************************/
28 #include "cvdrawingutils.h"
29 #include <opencv2/highgui/highgui.hpp>
30 #include <opencv2/calib3d/calib3d.hpp>
31 #include <opencv2/imgproc/imgproc.hpp>
32 
33 using namespace cv;
34 namespace aruco {
35 /****
36  *
37  *
38  *
39  ****/
40 void CvDrawingUtils::draw3dAxis(cv::Mat &Image, Marker &m, const CameraParameters &CP) {
41 
42  float size = m.ssize * 3;
43  Mat objectPoints(4, 3, CV_32FC1);
44  objectPoints.at< float >(0, 0) = 0;
45  objectPoints.at< float >(0, 1) = 0;
46  objectPoints.at< float >(0, 2) = 0;
47  objectPoints.at< float >(1, 0) = size;
48  objectPoints.at< float >(1, 1) = 0;
49  objectPoints.at< float >(1, 2) = 0;
50  objectPoints.at< float >(2, 0) = 0;
51  objectPoints.at< float >(2, 1) = size;
52  objectPoints.at< float >(2, 2) = 0;
53  objectPoints.at< float >(3, 0) = 0;
54  objectPoints.at< float >(3, 1) = 0;
55  objectPoints.at< float >(3, 2) = size;
56 
57  vector< Point2f > imagePoints;
58  cv::projectPoints(objectPoints, m.Rvec, m.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints);
59  // draw lines of different colours
60  cv::line(Image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255, 255), 1, CV_AA);
61  cv::line(Image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0, 255), 1, CV_AA);
62  cv::line(Image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0, 255), 1, CV_AA);
63  putText(Image, "x", imagePoints[1], FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 0, 255, 255), 2);
64  putText(Image, "y", imagePoints[2], FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0, 255), 2);
65  putText(Image, "z", imagePoints[3], FONT_HERSHEY_SIMPLEX, 0.6, Scalar(255, 0, 0, 255), 2);
66 }
67 
68 /****
69  *
70  *
71  *
72  ****/
73 void CvDrawingUtils::draw3dCube(cv::Mat &Image, Marker &m, const CameraParameters &CP, bool setYperpendicular) {
74  Mat objectPoints(8, 3, CV_32FC1);
75  double halfSize = m.ssize / 2;
76 
77  if (setYperpendicular) {
78  objectPoints.at< float >(0, 0) = -halfSize;
79  objectPoints.at< float >(0, 1) = 0;
80  objectPoints.at< float >(0, 2) = -halfSize;
81  objectPoints.at< float >(1, 0) = halfSize;
82  objectPoints.at< float >(1, 1) = 0;
83  objectPoints.at< float >(1, 2) = -halfSize;
84  objectPoints.at< float >(2, 0) = halfSize;
85  objectPoints.at< float >(2, 1) = 0;
86  objectPoints.at< float >(2, 2) = halfSize;
87  objectPoints.at< float >(3, 0) = -halfSize;
88  objectPoints.at< float >(3, 1) = 0;
89  objectPoints.at< float >(3, 2) = halfSize;
90 
91  objectPoints.at< float >(4, 0) = -halfSize;
92  objectPoints.at< float >(4, 1) = m.ssize;
93  objectPoints.at< float >(4, 2) = -halfSize;
94  objectPoints.at< float >(5, 0) = halfSize;
95  objectPoints.at< float >(5, 1) = m.ssize;
96  objectPoints.at< float >(5, 2) = -halfSize;
97  objectPoints.at< float >(6, 0) = halfSize;
98  objectPoints.at< float >(6, 1) = m.ssize;
99  objectPoints.at< float >(6, 2) = halfSize;
100  objectPoints.at< float >(7, 0) = -halfSize;
101  objectPoints.at< float >(7, 1) = m.ssize;
102  objectPoints.at< float >(7, 2) = halfSize;
103  } else {
104  objectPoints.at< float >(0, 0) = -halfSize;
105  objectPoints.at< float >(0, 1) = -halfSize;
106  objectPoints.at< float >(0, 2) = 0;
107  objectPoints.at< float >(1, 0) = halfSize;
108  objectPoints.at< float >(1, 1) = -halfSize;
109  objectPoints.at< float >(1, 2) = 0;
110  objectPoints.at< float >(2, 0) = halfSize;
111  objectPoints.at< float >(2, 1) = halfSize;
112  objectPoints.at< float >(2, 2) = 0;
113  objectPoints.at< float >(3, 0) = -halfSize;
114  objectPoints.at< float >(3, 1) = halfSize;
115  objectPoints.at< float >(3, 2) = 0;
116 
117  objectPoints.at< float >(4, 0) = -halfSize;
118  objectPoints.at< float >(4, 1) = -halfSize;
119  objectPoints.at< float >(4, 2) = m.ssize;
120  objectPoints.at< float >(5, 0) = halfSize;
121  objectPoints.at< float >(5, 1) = -halfSize;
122  objectPoints.at< float >(5, 2) = m.ssize;
123  objectPoints.at< float >(6, 0) = halfSize;
124  objectPoints.at< float >(6, 1) = halfSize;
125  objectPoints.at< float >(6, 2) = m.ssize;
126  objectPoints.at< float >(7, 0) = -halfSize;
127  objectPoints.at< float >(7, 1) = halfSize;
128  objectPoints.at< float >(7, 2) = m.ssize;
129  }
130 
131  vector< Point2f > imagePoints;
132  projectPoints(objectPoints, m.Rvec, m.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints);
133  // draw lines of different colours
134  for (int i = 0; i < 4; i++)
135  cv::line(Image, imagePoints[i], imagePoints[(i + 1) % 4], Scalar(0, 0, 255, 255), 1, CV_AA);
136 
137  for (int i = 0; i < 4; i++)
138  cv::line(Image, imagePoints[i + 4], imagePoints[4 + (i + 1) % 4], Scalar(0, 0, 255, 255), 1, CV_AA);
139 
140  for (int i = 0; i < 4; i++)
141  cv::line(Image, imagePoints[i], imagePoints[i + 4], Scalar(0, 0, 255, 255), 1, CV_AA);
142 }
143 
144 
145 /****
146  *
147  *
148  *
149  ****/
150 void CvDrawingUtils::draw3dAxis(cv::Mat &Image, Board &B, const CameraParameters &CP) {
151  Mat objectPoints(4, 3, CV_32FC1);
152  objectPoints.at< float >(0, 0) = 0;
153  objectPoints.at< float >(0, 1) = 0;
154  objectPoints.at< float >(0, 2) = 0;
155  objectPoints.at< float >(1, 0) = 2 * B[0].ssize;
156  objectPoints.at< float >(1, 1) = 0;
157  objectPoints.at< float >(1, 2) = 0;
158  objectPoints.at< float >(2, 0) = 0;
159  objectPoints.at< float >(2, 1) = 2 * B[0].ssize;
160  objectPoints.at< float >(2, 2) = 0;
161  objectPoints.at< float >(3, 0) = 0;
162  objectPoints.at< float >(3, 1) = 0;
163  objectPoints.at< float >(3, 2) = 2 * B[0].ssize;
164 
165  vector< Point2f > imagePoints;
166  projectPoints(objectPoints, B.Rvec, B.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints);
167  // draw lines of different colours
168  cv::line(Image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255, 255), 2, CV_AA);
169  cv::line(Image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0, 255), 2, CV_AA);
170  cv::line(Image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0, 255), 2, CV_AA);
171 
172  putText(Image, "X", imagePoints[1], FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255, 255), 2);
173  putText(Image, "Y", imagePoints[2], FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0, 255), 2);
174  putText(Image, "Z", imagePoints[3], FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 0, 0, 255), 2);
175 }
176 
177 
178 /****
179  *
180  *
181  *
182  ****/
183 void CvDrawingUtils::draw3dCube(cv::Mat &Image, Board &B, const CameraParameters &CP, bool setYperpendicular) {
184 
185  float cubeSize = B[0].ssize;
186  float txz = -cubeSize / 2;
187  Mat objectPoints(8, 3, CV_32FC1);
188 
189  if (setYperpendicular) {
190  objectPoints.at< float >(0, 0) = txz;
191  objectPoints.at< float >(0, 1) = 0;
192  objectPoints.at< float >(0, 2) = txz;
193  objectPoints.at< float >(1, 0) = txz + cubeSize;
194  objectPoints.at< float >(1, 1) = 0;
195  objectPoints.at< float >(1, 2) = txz;
196  objectPoints.at< float >(2, 0) = txz + cubeSize;
197  objectPoints.at< float >(2, 1) = cubeSize;
198  objectPoints.at< float >(2, 2) = txz;
199  objectPoints.at< float >(3, 0) = txz;
200  objectPoints.at< float >(3, 1) = cubeSize;
201  objectPoints.at< float >(3, 2) = txz;
202 
203  objectPoints.at< float >(4, 0) = txz;
204  objectPoints.at< float >(4, 1) = 0;
205  objectPoints.at< float >(4, 2) = txz + cubeSize;
206  objectPoints.at< float >(5, 0) = txz + cubeSize;
207  objectPoints.at< float >(5, 1) = 0;
208  objectPoints.at< float >(5, 2) = txz + cubeSize;
209  objectPoints.at< float >(6, 0) = txz + cubeSize;
210  objectPoints.at< float >(6, 1) = cubeSize;
211  objectPoints.at< float >(6, 2) = txz + cubeSize;
212  objectPoints.at< float >(7, 0) = txz;
213  objectPoints.at< float >(7, 1) = cubeSize;
214  objectPoints.at< float >(7, 2) = txz + cubeSize;
215  } else {
216  objectPoints.at< float >(0, 0) = txz;
217  objectPoints.at< float >(0, 2) = 0;
218  objectPoints.at< float >(0, 1) = txz;
219  objectPoints.at< float >(1, 0) = txz + cubeSize;
220  objectPoints.at< float >(1, 2) = 0;
221  objectPoints.at< float >(1, 1) = txz;
222  objectPoints.at< float >(2, 0) = txz + cubeSize;
223  objectPoints.at< float >(2, 2) = -cubeSize;
224  objectPoints.at< float >(2, 1) = txz;
225  objectPoints.at< float >(3, 0) = txz;
226  objectPoints.at< float >(3, 2) = -cubeSize;
227  objectPoints.at< float >(3, 1) = txz;
228 
229  objectPoints.at< float >(4, 0) = txz;
230  objectPoints.at< float >(4, 2) = 0;
231  objectPoints.at< float >(4, 1) = txz + cubeSize;
232  objectPoints.at< float >(5, 0) = txz + cubeSize;
233  objectPoints.at< float >(5, 2) = 0;
234  objectPoints.at< float >(5, 1) = txz + cubeSize;
235  objectPoints.at< float >(6, 0) = txz + cubeSize;
236  objectPoints.at< float >(6, 2) = -cubeSize;
237  objectPoints.at< float >(6, 1) = txz + cubeSize;
238  objectPoints.at< float >(7, 0) = txz;
239  objectPoints.at< float >(7, 2) = -cubeSize;
240  objectPoints.at< float >(7, 1) = txz + cubeSize;
241  }
242 
243  vector< Point2f > imagePoints;
244  projectPoints(objectPoints, B.Rvec, B.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints);
245  // draw lines of different colours
246  for (int i = 0; i < 4; i++)
247  cv::line(Image, imagePoints[i], imagePoints[(i + 1) % 4], Scalar(0, 0, 255, 255), 1, CV_AA);
248 
249  for (int i = 0; i < 4; i++)
250  cv::line(Image, imagePoints[i + 4], imagePoints[4 + (i + 1) % 4], Scalar(0, 0, 255, 255), 1, CV_AA);
251 
252  for (int i = 0; i < 4; i++)
253  cv::line(Image, imagePoints[i], imagePoints[i + 4], Scalar(0, 0, 255, 255), 1, CV_AA);
254 }
255 }
aruco::Board::Tvec
cv::Mat Tvec
Definition: board.h:133
aruco::Marker::Rvec
cv::Mat Rvec
Definition: marker.h:48
aruco::Marker
This class represents a marker. It is a vector of the fours corners ot the marker.
Definition: marker.h:41
aruco::Board::Rvec
cv::Mat Rvec
Definition: board.h:133
aruco::CameraParameters
Parameters of the camera.
Definition: cameraparameters.h:38
aruco::Marker::ssize
float ssize
Definition: marker.h:46
aruco::Marker::Tvec
cv::Mat Tvec
Definition: marker.h:48

asr_aruco_marker_recognition
Author(s): Allgeyer Tobias, Meißner Pascal, Qattan Mohamad
autogenerated on Mon Feb 28 2022 21:41:30