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