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
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
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
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
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 }