aruco_pose: cvdrawingutils.cpp Source File

Go to the documentation of this file.
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 }