asr_aruco_marker_recognition: cvdrawingutils.cpp Source File

Go to the documentation of this file.
00001 /*****************************
00002 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
00003 
00004 Redistribution and use in source and binary forms, with or without modification, are
00005 permitted provided that the following conditions are met:
00006 
00007    1. Redistributions of source code must retain the above copyright notice, this list of
00008       conditions and the following disclaimer.
00009 
00010    2. Redistributions in binary form must reproduce the above copyright notice, this list
00011       of conditions and the following disclaimer in the documentation and/or other materials
00012       provided with the distribution.
00013 
00014 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
00015 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00016 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
00017 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00018 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00019 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
00020 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00021 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00022 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00023 
00024 The views and conclusions contained in the software and documentation are those of the
00025 authors and should not be interpreted as representing official policies, either expressed
00026 or implied, of Rafael Muñoz Salinas.
00027 ********************************/
00028 #include "cvdrawingutils.h"
00029 #include <opencv2/highgui/highgui.hpp>
00030 #include <opencv2/calib3d/calib3d.hpp>
00031 #include <opencv2/imgproc/imgproc.hpp>
00032 
00033 using namespace cv;
00034 namespace aruco {
00035 /****
00036  *
00037  *
00038  *
00039  ****/
00040 void CvDrawingUtils::draw3dAxis(cv::Mat &Image, Marker &m, const CameraParameters &CP) {
00041 
00042     float size = m.ssize * 3;
00043     Mat objectPoints(4, 3, CV_32FC1);
00044     objectPoints.at< float >(0, 0) = 0;
00045     objectPoints.at< float >(0, 1) = 0;
00046     objectPoints.at< float >(0, 2) = 0;
00047     objectPoints.at< float >(1, 0) = size;
00048     objectPoints.at< float >(1, 1) = 0;
00049     objectPoints.at< float >(1, 2) = 0;
00050     objectPoints.at< float >(2, 0) = 0;
00051     objectPoints.at< float >(2, 1) = size;
00052     objectPoints.at< float >(2, 2) = 0;
00053     objectPoints.at< float >(3, 0) = 0;
00054     objectPoints.at< float >(3, 1) = 0;
00055     objectPoints.at< float >(3, 2) = size;
00056 
00057     vector< Point2f > imagePoints;
00058     cv::projectPoints(objectPoints, m.Rvec, m.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints);
00059     // draw lines of different colours
00060     cv::line(Image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255, 255), 1, CV_AA);
00061     cv::line(Image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0, 255), 1, CV_AA);
00062     cv::line(Image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0, 255), 1, CV_AA);
00063     putText(Image, "x", imagePoints[1], FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 0, 255, 255), 2);
00064     putText(Image, "y", imagePoints[2], FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0, 255), 2);
00065     putText(Image, "z", imagePoints[3], FONT_HERSHEY_SIMPLEX, 0.6, Scalar(255, 0, 0, 255), 2);
00066 }
00067 
00068 /****
00069  *
00070  *
00071  *
00072  ****/
00073 void CvDrawingUtils::draw3dCube(cv::Mat &Image, Marker &m, const CameraParameters &CP, bool setYperpendicular) {
00074     Mat objectPoints(8, 3, CV_32FC1);
00075     double halfSize = m.ssize / 2;
00076 
00077     if (setYperpendicular) {
00078         objectPoints.at< float >(0, 0) = -halfSize;
00079         objectPoints.at< float >(0, 1) = 0;
00080         objectPoints.at< float >(0, 2) = -halfSize;
00081         objectPoints.at< float >(1, 0) = halfSize;
00082         objectPoints.at< float >(1, 1) = 0;
00083         objectPoints.at< float >(1, 2) = -halfSize;
00084         objectPoints.at< float >(2, 0) = halfSize;
00085         objectPoints.at< float >(2, 1) = 0;
00086         objectPoints.at< float >(2, 2) = halfSize;
00087         objectPoints.at< float >(3, 0) = -halfSize;
00088         objectPoints.at< float >(3, 1) = 0;
00089         objectPoints.at< float >(3, 2) = halfSize;
00090 
00091         objectPoints.at< float >(4, 0) = -halfSize;
00092         objectPoints.at< float >(4, 1) = m.ssize;
00093         objectPoints.at< float >(4, 2) = -halfSize;
00094         objectPoints.at< float >(5, 0) = halfSize;
00095         objectPoints.at< float >(5, 1) = m.ssize;
00096         objectPoints.at< float >(5, 2) = -halfSize;
00097         objectPoints.at< float >(6, 0) = halfSize;
00098         objectPoints.at< float >(6, 1) = m.ssize;
00099         objectPoints.at< float >(6, 2) = halfSize;
00100         objectPoints.at< float >(7, 0) = -halfSize;
00101         objectPoints.at< float >(7, 1) = m.ssize;
00102         objectPoints.at< float >(7, 2) = halfSize;
00103     } else {
00104         objectPoints.at< float >(0, 0) = -halfSize;
00105         objectPoints.at< float >(0, 1) = -halfSize;
00106         objectPoints.at< float >(0, 2) = 0;
00107         objectPoints.at< float >(1, 0) = halfSize;
00108         objectPoints.at< float >(1, 1) = -halfSize;
00109         objectPoints.at< float >(1, 2) = 0;
00110         objectPoints.at< float >(2, 0) = halfSize;
00111         objectPoints.at< float >(2, 1) = halfSize;
00112         objectPoints.at< float >(2, 2) = 0;
00113         objectPoints.at< float >(3, 0) = -halfSize;
00114         objectPoints.at< float >(3, 1) = halfSize;
00115         objectPoints.at< float >(3, 2) = 0;
00116 
00117         objectPoints.at< float >(4, 0) = -halfSize;
00118         objectPoints.at< float >(4, 1) = -halfSize;
00119         objectPoints.at< float >(4, 2) = m.ssize;
00120         objectPoints.at< float >(5, 0) = halfSize;
00121         objectPoints.at< float >(5, 1) = -halfSize;
00122         objectPoints.at< float >(5, 2) = m.ssize;
00123         objectPoints.at< float >(6, 0) = halfSize;
00124         objectPoints.at< float >(6, 1) = halfSize;
00125         objectPoints.at< float >(6, 2) = m.ssize;
00126         objectPoints.at< float >(7, 0) = -halfSize;
00127         objectPoints.at< float >(7, 1) = halfSize;
00128         objectPoints.at< float >(7, 2) = m.ssize;
00129     }
00130 
00131     vector< Point2f > imagePoints;
00132     projectPoints(objectPoints, m.Rvec, m.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints);
00133     // draw lines of different colours
00134     for (int i = 0; i < 4; i++)
00135         cv::line(Image, imagePoints[i], imagePoints[(i + 1) % 4], Scalar(0, 0, 255, 255), 1, CV_AA);
00136 
00137     for (int i = 0; i < 4; i++)
00138         cv::line(Image, imagePoints[i + 4], imagePoints[4 + (i + 1) % 4], Scalar(0, 0, 255, 255), 1, CV_AA);
00139 
00140     for (int i = 0; i < 4; i++)
00141         cv::line(Image, imagePoints[i], imagePoints[i + 4], Scalar(0, 0, 255, 255), 1, CV_AA);
00142 }
00143 
00144 
00145 /****
00146  *
00147  *
00148  *
00149  ****/
00150 void CvDrawingUtils::draw3dAxis(cv::Mat &Image, Board &B, const CameraParameters &CP) {
00151     Mat objectPoints(4, 3, CV_32FC1);
00152     objectPoints.at< float >(0, 0) = 0;
00153     objectPoints.at< float >(0, 1) = 0;
00154     objectPoints.at< float >(0, 2) = 0;
00155     objectPoints.at< float >(1, 0) = 2 * B[0].ssize;
00156     objectPoints.at< float >(1, 1) = 0;
00157     objectPoints.at< float >(1, 2) = 0;
00158     objectPoints.at< float >(2, 0) = 0;
00159     objectPoints.at< float >(2, 1) = 2 * B[0].ssize;
00160     objectPoints.at< float >(2, 2) = 0;
00161     objectPoints.at< float >(3, 0) = 0;
00162     objectPoints.at< float >(3, 1) = 0;
00163     objectPoints.at< float >(3, 2) = 2 * B[0].ssize;
00164 
00165     vector< Point2f > imagePoints;
00166     projectPoints(objectPoints, B.Rvec, B.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints);
00167     // draw lines of different colours
00168     cv::line(Image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255, 255), 2, CV_AA);
00169     cv::line(Image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0, 255), 2, CV_AA);
00170     cv::line(Image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0, 255), 2, CV_AA);
00171 
00172     putText(Image, "X", imagePoints[1], FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255, 255), 2);
00173     putText(Image, "Y", imagePoints[2], FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0, 255), 2);
00174     putText(Image, "Z", imagePoints[3], FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 0, 0, 255), 2);
00175 }
00176 
00177 
00178 /****
00179  *
00180  *
00181  *
00182  ****/
00183 void CvDrawingUtils::draw3dCube(cv::Mat &Image, Board &B, const CameraParameters &CP, bool setYperpendicular) {
00184 
00185     float cubeSize = B[0].ssize;
00186     float txz = -cubeSize / 2;
00187     Mat objectPoints(8, 3, CV_32FC1);
00188 
00189     if (setYperpendicular) {
00190         objectPoints.at< float >(0, 0) = txz;
00191         objectPoints.at< float >(0, 1) = 0;
00192         objectPoints.at< float >(0, 2) = txz;
00193         objectPoints.at< float >(1, 0) = txz + cubeSize;
00194         objectPoints.at< float >(1, 1) = 0;
00195         objectPoints.at< float >(1, 2) = txz;
00196         objectPoints.at< float >(2, 0) = txz + cubeSize;
00197         objectPoints.at< float >(2, 1) = cubeSize;
00198         objectPoints.at< float >(2, 2) = txz;
00199         objectPoints.at< float >(3, 0) = txz;
00200         objectPoints.at< float >(3, 1) = cubeSize;
00201         objectPoints.at< float >(3, 2) = txz;
00202 
00203         objectPoints.at< float >(4, 0) = txz;
00204         objectPoints.at< float >(4, 1) = 0;
00205         objectPoints.at< float >(4, 2) = txz + cubeSize;
00206         objectPoints.at< float >(5, 0) = txz + cubeSize;
00207         objectPoints.at< float >(5, 1) = 0;
00208         objectPoints.at< float >(5, 2) = txz + cubeSize;
00209         objectPoints.at< float >(6, 0) = txz + cubeSize;
00210         objectPoints.at< float >(6, 1) = cubeSize;
00211         objectPoints.at< float >(6, 2) = txz + cubeSize;
00212         objectPoints.at< float >(7, 0) = txz;
00213         objectPoints.at< float >(7, 1) = cubeSize;
00214         objectPoints.at< float >(7, 2) = txz + cubeSize;
00215     } else {
00216         objectPoints.at< float >(0, 0) = txz;
00217         objectPoints.at< float >(0, 2) = 0;
00218         objectPoints.at< float >(0, 1) = txz;
00219         objectPoints.at< float >(1, 0) = txz + cubeSize;
00220         objectPoints.at< float >(1, 2) = 0;
00221         objectPoints.at< float >(1, 1) = txz;
00222         objectPoints.at< float >(2, 0) = txz + cubeSize;
00223         objectPoints.at< float >(2, 2) = -cubeSize;
00224         objectPoints.at< float >(2, 1) = txz;
00225         objectPoints.at< float >(3, 0) = txz;
00226         objectPoints.at< float >(3, 2) = -cubeSize;
00227         objectPoints.at< float >(3, 1) = txz;
00228 
00229         objectPoints.at< float >(4, 0) = txz;
00230         objectPoints.at< float >(4, 2) = 0;
00231         objectPoints.at< float >(4, 1) = txz + cubeSize;
00232         objectPoints.at< float >(5, 0) = txz + cubeSize;
00233         objectPoints.at< float >(5, 2) = 0;
00234         objectPoints.at< float >(5, 1) = txz + cubeSize;
00235         objectPoints.at< float >(6, 0) = txz + cubeSize;
00236         objectPoints.at< float >(6, 2) = -cubeSize;
00237         objectPoints.at< float >(6, 1) = txz + cubeSize;
00238         objectPoints.at< float >(7, 0) = txz;
00239         objectPoints.at< float >(7, 2) = -cubeSize;
00240         objectPoints.at< float >(7, 1) = txz + cubeSize;
00241     }
00242 
00243     vector< Point2f > imagePoints;
00244     projectPoints(objectPoints, B.Rvec, B.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints);
00245     // draw lines of different colours
00246     for (int i = 0; i < 4; i++)
00247         cv::line(Image, imagePoints[i], imagePoints[(i + 1) % 4], Scalar(0, 0, 255, 255), 1, CV_AA);
00248 
00249     for (int i = 0; i < 4; i++)
00250         cv::line(Image, imagePoints[i + 4], imagePoints[4 + (i + 1) % 4], Scalar(0, 0, 255, 255), 1, CV_AA);
00251 
00252     for (int i = 0; i < 4; i++)
00253         cv::line(Image, imagePoints[i], imagePoints[i + 4], Scalar(0, 0, 255, 255), 1, CV_AA);
00254 }
00255 }