ar_sys: 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 <aruco/cvdrawingutils.h>
00029 #include <opencv2/highgui/highgui.hpp>
00030 #include <opencv2/calib3d/calib3d.hpp>
00031 using namespace cv;
00032 namespace aruco {
00033 /****
00034  *
00035  *
00036  *
00037  ****/
00038 void CvDrawingUtils::draw3dAxis(cv::Mat &Image,Marker &m,const CameraParameters &CP)
00039 {
00040 
00041   float size=m.ssize*3;
00042     Mat objectPoints (4,3,CV_32FC1);
00043     objectPoints.at<float>(0,0)=0;
00044     objectPoints.at<float>(0,1)=0;
00045     objectPoints.at<float>(0,2)=0;
00046     objectPoints.at<float>(1,0)=0;
00047     objectPoints.at<float>(1,1)=size;
00048     objectPoints.at<float>(1,2)=0;
00049     objectPoints.at<float>(2,0)=-size;
00050     objectPoints.at<float>(2,1)=0;
00051     objectPoints.at<float>(2,2)=0;
00052     objectPoints.at<float>(3,0)=0;
00053     objectPoints.at<float>(3,1)=0;
00054     objectPoints.at<float>(3,2)=size;
00055 
00056     vector<Point2f> imagePoints;
00057     cv::projectPoints( objectPoints, m.Rvec,m.Tvec, CP.CameraMatrix,CP.Distorsion,   imagePoints);
00058 //draw lines of different colours
00059     cv::line(Image,imagePoints[0],imagePoints[1],Scalar(255,0,0,255),1,CV_AA);
00060     cv::line(Image,imagePoints[0],imagePoints[2],Scalar(0,255,0,255),1,CV_AA);
00061     cv::line(Image,imagePoints[0],imagePoints[3],Scalar(0,0,255,255),1,CV_AA);
00062     putText(Image,"x", imagePoints[1],FONT_HERSHEY_SIMPLEX, 0.6, Scalar(255,0,0,255),2);
00063     putText(Image,"y", imagePoints[2],FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0,255,0,255),2);
00064     putText(Image,"z", imagePoints[3],FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0,0,255,255),2);
00065 }
00066 
00067 /****
00068  *
00069  *
00070  *
00071  ****/
00072 void CvDrawingUtils::draw3dCube(cv::Mat &Image,Marker &m,const CameraParameters &CP, bool setYperpendicular)
00073 {
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     }
00104     else {
00105       objectPoints.at<float>(0,0)=-halfSize;
00106       objectPoints.at<float>(0,1)=-halfSize;
00107       objectPoints.at<float>(0,2)=0;
00108       objectPoints.at<float>(1,0)=halfSize;
00109       objectPoints.at<float>(1,1)=-halfSize;
00110       objectPoints.at<float>(1,2)=0;
00111       objectPoints.at<float>(2,0)=halfSize;
00112       objectPoints.at<float>(2,1)=halfSize;
00113       objectPoints.at<float>(2,2)=0;
00114       objectPoints.at<float>(3,0)=-halfSize;
00115       objectPoints.at<float>(3,1)=halfSize;
00116       objectPoints.at<float>(3,2)=0;
00117 
00118       objectPoints.at<float>(4,0)=-halfSize;
00119       objectPoints.at<float>(4,1)=-halfSize;
00120       objectPoints.at<float>(4,2)=m.ssize;      
00121       objectPoints.at<float>(5,0)=halfSize;
00122       objectPoints.at<float>(5,1)=-halfSize;      
00123       objectPoints.at<float>(5,2)=m.ssize;
00124       objectPoints.at<float>(6,0)=halfSize;
00125       objectPoints.at<float>(6,1)=halfSize;      
00126       objectPoints.at<float>(6,2)=m.ssize;
00127       objectPoints.at<float>(7,0)=-halfSize;
00128       objectPoints.at<float>(7,1)=halfSize;         
00129       objectPoints.at<float>(7,2)=m.ssize;   
00130     }
00131 
00132     vector<Point2f> imagePoints;
00133     projectPoints( objectPoints, m.Rvec,m.Tvec,  CP.CameraMatrix,CP.Distorsion,   imagePoints);
00134 //draw lines of different colours
00135     for (int i=0;i<4;i++)
00136         cv::line(Image,imagePoints[i],imagePoints[(i+1)%4],Scalar(0,0,255,255),1,CV_AA);
00137 
00138     for (int i=0;i<4;i++)
00139         cv::line(Image,imagePoints[i+4],imagePoints[4+(i+1)%4],Scalar(0,0,255,255),1,CV_AA);
00140 
00141     for (int i=0;i<4;i++)
00142         cv::line(Image,imagePoints[i],imagePoints[i+4],Scalar(0,0,255,255),1,CV_AA);
00143 
00144 }
00145 
00146 
00147 /****
00148  *
00149  *
00150  *
00151  ****/
00152 void CvDrawingUtils::draw3dAxis(cv::Mat &Image,Board &B,const CameraParameters &CP)
00153 {
00154 Mat objectPoints (4,3,CV_32FC1);
00155 objectPoints.at<float>(0,0)=0;objectPoints.at<float>(0,1)=0;objectPoints.at<float>(0,2)=0;
00156 objectPoints.at<float>(1,0)=2*B[0].ssize;objectPoints.at<float>(1,1)=0;objectPoints.at<float>(1,2)=0;
00157 objectPoints.at<float>(2,0)=0;objectPoints.at<float>(2,1)=-2*B[0].ssize;objectPoints.at<float>(2,2)=0;
00158 objectPoints.at<float>(3,0)=0;objectPoints.at<float>(3,1)=0;objectPoints.at<float>(3,2)=-2*B[0].ssize;
00159 
00160 vector<Point2f> imagePoints;
00161 projectPoints( objectPoints, B.Rvec,B.Tvec, CP.CameraMatrix, CP.Distorsion,   imagePoints);
00162 //draw lines of different colours
00163 cv::line(Image,imagePoints[0],imagePoints[1],Scalar(255,0,0,255),2,CV_AA);
00164 cv::line(Image,imagePoints[0],imagePoints[2],Scalar(0,255,0,255),2,CV_AA);
00165 cv::line(Image,imagePoints[0],imagePoints[3],Scalar(0,0,255,255),2,CV_AA);
00166 
00167 putText(Image,"X", imagePoints[1],FONT_HERSHEY_SIMPLEX, 1, Scalar(255,0,0,255),2);
00168 putText(Image,"Y", imagePoints[2],FONT_HERSHEY_SIMPLEX, 1, Scalar(0,255,0,255),2);
00169 putText(Image,"Z", imagePoints[3],FONT_HERSHEY_SIMPLEX, 1, Scalar(0,0,255,255),2);
00170 }
00171 
00172 
00173 /****
00174  *
00175  *
00176  *
00177  ****/
00178 void CvDrawingUtils::draw3dCube(cv::Mat &Image,Board &B,const CameraParameters &CP, bool setYperpendicular)
00179 {
00180   
00181 float cubeSize=B[0].ssize;
00182 float txz=-cubeSize/2;
00183 Mat objectPoints (8,3,CV_32FC1);
00184 
00185 if(setYperpendicular) {
00186   objectPoints.at<float>(0,0)=txz;objectPoints.at<float>(0,1)=0;objectPoints.at<float>(0,2)=txz;
00187   objectPoints.at<float>(1,0)=txz+cubeSize;objectPoints.at<float>(1,1)=0;objectPoints.at<float>(1,2)=txz;
00188   objectPoints.at<float>(2,0)=txz+cubeSize;objectPoints.at<float>(2,1)=cubeSize;objectPoints.at<float>(2,2)=txz;
00189   objectPoints.at<float>(3,0)=txz;objectPoints.at<float>(3,1)=cubeSize;objectPoints.at<float>(3,2)=txz;
00190 
00191   objectPoints.at<float>(4,0)=txz;objectPoints.at<float>(4,1)=0;objectPoints.at<float>(4,2)=txz+cubeSize;
00192   objectPoints.at<float>(5,0)=txz+cubeSize;objectPoints.at<float>(5,1)=0;objectPoints.at<float>(5,2)=txz+cubeSize;
00193   objectPoints.at<float>(6,0)=txz+cubeSize;objectPoints.at<float>(6,1)=cubeSize;objectPoints.at<float>(6,2)=txz+cubeSize;
00194   objectPoints.at<float>(7,0)=txz;objectPoints.at<float>(7,1)=cubeSize;objectPoints.at<float>(7,2)=txz+cubeSize;
00195 }
00196 else {
00197   objectPoints.at<float>(0,0)=txz;objectPoints.at<float>(0,2)=0;objectPoints.at<float>(0,1)=txz;
00198   objectPoints.at<float>(1,0)=txz+cubeSize;objectPoints.at<float>(1,2)=0;objectPoints.at<float>(1,1)=txz;
00199   objectPoints.at<float>(2,0)=txz+cubeSize;objectPoints.at<float>(2,2)=-cubeSize;objectPoints.at<float>(2,1)=txz;
00200   objectPoints.at<float>(3,0)=txz;objectPoints.at<float>(3,2)=-cubeSize;objectPoints.at<float>(3,1)=txz;
00201 
00202   objectPoints.at<float>(4,0)=txz;objectPoints.at<float>(4,2)=0;objectPoints.at<float>(4,1)=txz+cubeSize;
00203   objectPoints.at<float>(5,0)=txz+cubeSize;objectPoints.at<float>(5,2)=0;objectPoints.at<float>(5,1)=txz+cubeSize;
00204   objectPoints.at<float>(6,0)=txz+cubeSize;objectPoints.at<float>(6,2)=-cubeSize;objectPoints.at<float>(6,1)=txz+cubeSize;
00205   objectPoints.at<float>(7,0)=txz;objectPoints.at<float>(7,2)=-cubeSize;objectPoints.at<float>(7,1)=txz+cubeSize;  
00206 }
00207 
00208 vector<Point2f> imagePoints;
00209 projectPoints( objectPoints,B.Rvec,B.Tvec, CP.CameraMatrix, CP.Distorsion,   imagePoints);
00210 //draw lines of different colours
00211 for(int i=0;i<4;i++)
00212   cv::line(Image,imagePoints[i],imagePoints[(i+1)%4],Scalar(0,0,255,255),1,CV_AA);
00213 
00214 for(int i=0;i<4;i++)
00215   cv::line(Image,imagePoints[i+4],imagePoints[4+(i+1)%4],Scalar(0,0,255,255),1,CV_AA);
00216 
00217 for(int i=0;i<4;i++)
00218   cv::line(Image,imagePoints[i],imagePoints[i+4],Scalar(0,0,255,255),1,CV_AA);
00219 }
00220 
00221 }