00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
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
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
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
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
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
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
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 }