$search
00001 #include "aruco/cvdrawingutils.h" 00002 00003 namespace aruco { 00004 /**** 00005 * 00006 * 00007 * 00008 ****/ 00009 void CvDrawingUtils::draw3dAxis(cv::Mat &Image,Marker &m,CameraParameters &CP) 00010 { 00011 00012 Mat objectPoints (4,3,CV_32FC1); 00013 objectPoints.at<float>(0,0)=0; 00014 objectPoints.at<float>(0,1)=0; 00015 objectPoints.at<float>(0,2)=0; 00016 objectPoints.at<float>(1,0)=m.ssize; 00017 objectPoints.at<float>(1,1)=0; 00018 objectPoints.at<float>(1,2)=0; 00019 objectPoints.at<float>(2,0)=0; 00020 objectPoints.at<float>(2,1)=m.ssize; 00021 objectPoints.at<float>(2,2)=0; 00022 objectPoints.at<float>(3,0)=0; 00023 objectPoints.at<float>(3,1)=0; 00024 objectPoints.at<float>(3,2)=m.ssize; 00025 00026 vector<Point2f> imagePoints; 00027 cv::projectPoints( objectPoints, m.Rvec,m.Tvec, CP.CameraMatrix,CP.Distorsion, imagePoints); 00028 //draw lines of different colours 00029 cv::line(Image,imagePoints[0],imagePoints[1],Scalar(0,0,255),1,CV_AA); 00030 cv::line(Image,imagePoints[0],imagePoints[2],Scalar(0,255,0),1,CV_AA); 00031 cv::line(Image,imagePoints[0],imagePoints[3],Scalar(255,0,0),1,CV_AA); 00032 putText(Image,"x", imagePoints[1],FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0,0,255),2); 00033 putText(Image,"y", imagePoints[2],FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0,255,0),2); 00034 putText(Image,"z", imagePoints[3],FONT_HERSHEY_SIMPLEX, 0.6, Scalar(255,0,0),2); 00035 } 00036 00037 /**** 00038 * 00039 * 00040 * 00041 ****/ 00042 void CvDrawingUtils::draw3dCube(cv::Mat &Image,Marker &m,CameraParameters &CP) 00043 { 00044 Mat objectPoints (8,3,CV_32FC1); 00045 double halfSize=m.ssize/2; 00046 objectPoints.at<float>(0,0)=-halfSize; 00047 objectPoints.at<float>(0,1)=0; 00048 objectPoints.at<float>(0,2)=-halfSize; 00049 objectPoints.at<float>(1,0)=halfSize; 00050 objectPoints.at<float>(1,1)=0; 00051 objectPoints.at<float>(1,2)=-halfSize; 00052 objectPoints.at<float>(2,0)=halfSize; 00053 objectPoints.at<float>(2,1)=0; 00054 objectPoints.at<float>(2,2)=halfSize; 00055 objectPoints.at<float>(3,0)=-halfSize; 00056 objectPoints.at<float>(3,1)=0; 00057 objectPoints.at<float>(3,2)=halfSize; 00058 00059 objectPoints.at<float>(4,0)=-halfSize; 00060 objectPoints.at<float>(4,1)=m.ssize; 00061 objectPoints.at<float>(4,2)=-halfSize; 00062 objectPoints.at<float>(5,0)=halfSize; 00063 objectPoints.at<float>(5,1)=m.ssize; 00064 objectPoints.at<float>(5,2)=-halfSize; 00065 objectPoints.at<float>(6,0)=halfSize; 00066 objectPoints.at<float>(6,1)=m.ssize; 00067 objectPoints.at<float>(6,2)=halfSize; 00068 objectPoints.at<float>(7,0)=-halfSize; 00069 objectPoints.at<float>(7,1)=m.ssize; 00070 objectPoints.at<float>(7,2)=halfSize; 00071 00072 vector<Point2f> imagePoints; 00073 projectPoints( objectPoints, m.Rvec,m.Tvec, CP.CameraMatrix,CP.Distorsion, imagePoints); 00074 //draw lines of different colours 00075 for (int i=0;i<4;i++) 00076 cv::line(Image,imagePoints[i],imagePoints[(i+1)%4],Scalar(0,0,255),1,CV_AA); 00077 00078 for (int i=0;i<4;i++) 00079 cv::line(Image,imagePoints[i+4],imagePoints[4+(i+1)%4],Scalar(0,0,255),1,CV_AA); 00080 00081 for (int i=0;i<4;i++) 00082 cv::line(Image,imagePoints[i],imagePoints[i+4],Scalar(0,0,255),1,CV_AA); 00083 00084 } 00085 00086 00087 /**** 00088 * 00089 * 00090 * 00091 ****/ 00092 void CvDrawingUtils::draw3dAxis(cv::Mat &Image,Board &B,CameraParameters &CP) 00093 { 00094 Mat objectPoints (4,3,CV_32FC1); 00095 objectPoints.at<float>(0,0)=0;objectPoints.at<float>(0,1)=0;objectPoints.at<float>(0,2)=0; 00096 objectPoints.at<float>(1,0)=2*B[0].ssize;objectPoints.at<float>(1,1)=0;objectPoints.at<float>(1,2)=0; 00097 objectPoints.at<float>(2,0)=0;objectPoints.at<float>(2,1)=2*B[0].ssize;objectPoints.at<float>(2,2)=0; 00098 objectPoints.at<float>(3,0)=0;objectPoints.at<float>(3,1)=0;objectPoints.at<float>(3,2)=2*B[0].ssize; 00099 00100 vector<Point2f> imagePoints; 00101 projectPoints( objectPoints, B.Rvec,B.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints); 00102 //draw lines of different colours 00103 cv::line(Image,imagePoints[0],imagePoints[1],Scalar(0,0,255),2,CV_AA); 00104 cv::line(Image,imagePoints[0],imagePoints[2],Scalar(0,255,0),2,CV_AA); 00105 cv::line(Image,imagePoints[0],imagePoints[3],Scalar(255,0,0),2,CV_AA); 00106 00107 putText(Image,"X", imagePoints[1],FONT_HERSHEY_SIMPLEX, 1, Scalar(0,0,255),2); 00108 putText(Image,"Y", imagePoints[2],FONT_HERSHEY_SIMPLEX, 1, Scalar(0,255,0),2); 00109 putText(Image,"Z", imagePoints[3],FONT_HERSHEY_SIMPLEX, 1, Scalar(255,0,0),2); 00110 } 00111 00112 00113 /**** 00114 * 00115 * 00116 * 00117 ****/ 00118 void CvDrawingUtils::draw3dCube(cv::Mat &Image,Board &B,CameraParameters &CP) 00119 { 00120 00121 float cubeSize=B[0].ssize; 00122 float txz=-cubeSize/2; 00123 Mat objectPoints (8,3,CV_32FC1); 00124 objectPoints.at<float>(0,0)=txz;objectPoints.at<float>(0,1)=0;objectPoints.at<float>(0,2)=txz; 00125 objectPoints.at<float>(1,0)=txz+cubeSize;objectPoints.at<float>(1,1)=0;objectPoints.at<float>(1,2)=txz; 00126 objectPoints.at<float>(2,0)=txz+cubeSize;objectPoints.at<float>(2,1)=cubeSize;objectPoints.at<float>(2,2)=txz; 00127 objectPoints.at<float>(3,0)=txz;objectPoints.at<float>(3,1)=cubeSize;objectPoints.at<float>(3,2)=txz; 00128 00129 objectPoints.at<float>(4,0)=txz;objectPoints.at<float>(4,1)=0;objectPoints.at<float>(4,2)=txz+cubeSize; 00130 objectPoints.at<float>(5,0)=txz+cubeSize;objectPoints.at<float>(5,1)=0;objectPoints.at<float>(5,2)=txz+cubeSize; 00131 objectPoints.at<float>(6,0)=txz+cubeSize;objectPoints.at<float>(6,1)=cubeSize;objectPoints.at<float>(6,2)=txz+cubeSize; 00132 objectPoints.at<float>(7,0)=txz;objectPoints.at<float>(7,1)=cubeSize;objectPoints.at<float>(7,2)=txz+cubeSize; 00133 00134 vector<Point2f> imagePoints; 00135 projectPoints( objectPoints,B.Rvec,B.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints); 00136 //draw lines of different colours 00137 for(int i=0;i<4;i++) 00138 cv::line(Image,imagePoints[i],imagePoints[(i+1)%4],Scalar(0,0,255),1,CV_AA); 00139 00140 for(int i=0;i<4;i++) 00141 cv::line(Image,imagePoints[i+4],imagePoints[4+(i+1)%4],Scalar(0,0,255),1,CV_AA); 00142 00143 for(int i=0;i<4;i++) 00144 cv::line(Image,imagePoints[i],imagePoints[i+4],Scalar(0,0,255),1,CV_AA); 00145 } 00146 00147 }