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