1 /* 2 * Utils.cpp 3 * 4 * Created on: Mar 28, 2014 5 * Author: Edgar Riba 6 */ 7 8 #include <iostream> 9 10 #include "PnPProblem.h" 11 #include "ModelRegistration.h" 12 #include "Utils.h" 13 14 #include <opencv2/imgproc/imgproc.hpp> 15 #include <opencv2/calib3d/calib3d.hpp> 16 17 // For text 18 int fontFace = cv::FONT_ITALIC; 19 double fontScale = 0.75; 20 int thickness_font = 2; 21 22 // For circles 23 int lineType = 8; 24 int radius = 4; 25 double thickness_circ = -1; 26 27 // Draw a text with the question point 28 void drawQuestion(cv::Mat image, cv::Point3f point, cv::Scalar color) 29 { 30 std::string x = IntToString((int)point.x); 31 std::string y = IntToString((int)point.y); 32 std::string z = IntToString((int)point.z); 33 34 std::string text = " Where is point (" + x + "," + y + "," + z + ") ?"; 35 cv::putText(image, text, cv::Point(25,50), fontFace, fontScale, color, thickness_font, 8); 36 } 37 38 // Draw a text with the number of entered points 39 void drawText(cv::Mat image, std::string text, cv::Scalar color) 40 { 41 cv::putText(image, text, cv::Point(25,50), fontFace, fontScale, color, thickness_font, 8); 42 } 43 44 // Draw a text with the number of entered points 45 void drawText2(cv::Mat image, std::string text, cv::Scalar color) 46 { 47 cv::putText(image, text, cv::Point(25,75), fontFace, fontScale, color, thickness_font, 8); 48 } 49 50 // Draw a text with the frame ratio 51 void drawFPS(cv::Mat image, double fps, cv::Scalar color) 52 { 53 std::string fps_str = IntToString((int)fps); 54 std::string text = fps_str + " FPS"; 55 cv::putText(image, text, cv::Point(500,50), fontFace, fontScale, color, thickness_font, 8); 56 } 57 58 // Draw a text with the frame ratio 59 void drawConfidence(cv::Mat image, double confidence, cv::Scalar color) 60 { 61 std::string conf_str = IntToString((int)confidence); 62 std::string text = conf_str + " %"; 63 cv::putText(image, text, cv::Point(500,75), fontFace, fontScale, color, thickness_font, 8); 64 } 65 66 // Draw a text with the number of entered points 67 void drawCounter(cv::Mat image, int n, int n_max, cv::Scalar color) 68 { 69 std::string n_str = IntToString(n); 70 std::string n_max_str = IntToString(n_max); 71 std::string text = n_str + " of " + n_max_str + " points"; 72 cv::putText(image, text, cv::Point(500,50), fontFace, fontScale, color, thickness_font, 8); 73 } 74 75 // Draw the points and the coordinates 76 void drawPoints(cv::Mat image, std::vector<cv::Point2f> &list_points_2d, std::vector<cv::Point3f> &list_points_3d, cv::Scalar color) 77 { 78 for (unsigned int i = 0; i < list_points_2d.size(); ++i) 79 { 80 cv::Point2f point_2d = list_points_2d[i]; 81 cv::Point3f point_3d = list_points_3d[i]; 82 83 // Draw Selected points 84 cv::circle(image, point_2d, radius, color, -1, lineType ); 85 86 std::string idx = IntToString(i+1); 87 std::string x = IntToString((int)point_3d.x); 88 std::string y = IntToString((int)point_3d.y); 89 std::string z = IntToString((int)point_3d.z); 90 std::string text = "P" + idx + " (" + x + "," + y + "," + z +")"; 91 92 point_2d.x = point_2d.x + 10; 93 point_2d.y = point_2d.y - 10; 94 cv::putText(image, text, point_2d, fontFace, fontScale*0.5, color, thickness_font, 8); 95 } 96 } 97 98 // Draw only the 2D points 99 void draw2DPoints(cv::Mat image, std::vector<cv::Point2f> &list_points, cv::Scalar color) 100 { 101 for( size_t i = 0; i < list_points.size(); i++) 102 { 103 cv::Point2f point_2d = list_points[i]; 104 105 // Draw Selected points 106 cv::circle(image, point_2d, radius, color, -1, lineType ); 107 } 108 } 109 110 // Draw an arrow into the image 111 void drawArrow(cv::Mat image, cv::Point2i p, cv::Point2i q, cv::Scalar color, int arrowMagnitude, int thickness, int line_type, int shift) 112 { 113 //Draw the principle line 114 cv::line(image, p, q, color, thickness, line_type, shift); 115 const double PI = CV_PI; 116 //compute the angle alpha 117 double angle = atan2((double)p.y-q.y, (double)p.x-q.x); 118 //compute the coordinates of the first segment 119 p.x = (int) ( q.x + arrowMagnitude * cos(angle + PI/4)); 120 p.y = (int) ( q.y + arrowMagnitude * sin(angle + PI/4)); 121 //Draw the first segment 122 cv::line(image, p, q, color, thickness, line_type, shift); 123 //compute the coordinates of the second segment 124 p.x = (int) ( q.x + arrowMagnitude * cos(angle - PI/4)); 125 p.y = (int) ( q.y + arrowMagnitude * sin(angle - PI/4)); 126 //Draw the second segment 127 cv::line(image, p, q, color, thickness, line_type, shift); 128 } 129 130 // Draw the 3D coordinate axes 131 void draw3DCoordinateAxes(cv::Mat image, const std::vector<cv::Point2f> &list_points2d) 132 { 133 cv::Scalar red(0, 0, 255); 134 cv::Scalar green(0,255,0); 135 cv::Scalar blue(255,0,0); 136 cv::Scalar black(0,0,0); 137 138 cv::Point2i origin = list_points2d[0]; 139 cv::Point2i pointX = list_points2d[1]; 140 cv::Point2i pointY = list_points2d[2]; 141 cv::Point2i pointZ = list_points2d[3]; 142 143 drawArrow(image, origin, pointX, red, 9, 2); 144 drawArrow(image, origin, pointY, blue, 9, 2); 145 drawArrow(image, origin, pointZ, green, 9, 2); 146 cv::circle(image, origin, radius/2, black, -1, lineType ); 147 148 } 149 150 // Draw the object mesh 151 void drawObjectMesh(cv::Mat image, const Mesh *mesh, PnPProblem *pnpProblem, cv::Scalar color) 152 { 153 std::vector<std::vector<int> > list_triangles = mesh->getTrianglesList(); 154 for( size_t i = 0; i < list_triangles.size(); i++) 155 { 156 std::vector<int> tmp_triangle = list_triangles.at(i); 157 158 cv::Point3f point_3d_0 = mesh->getVertex(tmp_triangle[0]); 159 cv::Point3f point_3d_1 = mesh->getVertex(tmp_triangle[1]); 160 cv::Point3f point_3d_2 = mesh->getVertex(tmp_triangle[2]); 161 162 cv::Point2f point_2d_0 = pnpProblem->backproject3DPoint(point_3d_0); 163 cv::Point2f point_2d_1 = pnpProblem->backproject3DPoint(point_3d_1); 164 cv::Point2f point_2d_2 = pnpProblem->backproject3DPoint(point_3d_2); 165 166 cv::line(image, point_2d_0, point_2d_1, color, 1); 167 cv::line(image, point_2d_1, point_2d_2, color, 1); 168 cv::line(image, point_2d_2, point_2d_0, color, 1); 169 } 170 } 171 172 // Computes the norm of the translation error 173 double get_translation_error(const cv::Mat &t_true, const cv::Mat &t) 174 { 175 return cv::norm( t_true - t ); 176 } 177 178 // Computes the norm of the rotation error 179 double get_rotation_error(const cv::Mat &R_true, const cv::Mat &R) 180 { 181 cv::Mat error_vec, error_mat; 182 error_mat = R_true * cv::Mat(R.inv()).mul(-1); 183 cv::Rodrigues(error_mat, error_vec); 184 185 return cv::norm(error_vec); 186 } 187 188 // Converts a given Rotation Matrix to Euler angles 189 cv::Mat rot2euler(const cv::Mat & rotationMatrix) 190 { 191 cv::Mat euler(3,1,CV_64F); 192 193 double m00 = rotationMatrix.at<double>(0,0); 194 double m02 = rotationMatrix.at<double>(0,2); 195 double m10 = rotationMatrix.at<double>(1,0); 196 double m11 = rotationMatrix.at<double>(1,1); 197 double m12 = rotationMatrix.at<double>(1,2); 198 double m20 = rotationMatrix.at<double>(2,0); 199 double m22 = rotationMatrix.at<double>(2,2); 200 201 double x, y, z; 202 203 // Assuming the angles are in radians. 204 if (m10 > 0.998) { // singularity at north pole 205 x = 0; 206 y = CV_PI/2; 207 z = atan2(m02,m22); 208 } 209 else if (m10 < -0.998) { // singularity at south pole 210 x = 0; 211 y = -CV_PI/2; 212 z = atan2(m02,m22); 213 } 214 else 215 { 216 x = atan2(-m12,m11); 217 y = asin(m10); 218 z = atan2(-m20,m00); 219 } 220 221 euler.at<double>(0) = x; 222 euler.at<double>(1) = y; 223 euler.at<double>(2) = z; 224 225 return euler; 226 } 227 228 // Converts a given Euler angles to Rotation Matrix 229 cv::Mat euler2rot(const cv::Mat & euler) 230 { 231 cv::Mat rotationMatrix(3,3,CV_64F); 232 233 double x = euler.at<double>(0); 234 double y = euler.at<double>(1); 235 double z = euler.at<double>(2); 236 237 // Assuming the angles are in radians. 238 double ch = cos(z); 239 double sh = sin(z); 240 double ca = cos(y); 241 double sa = sin(y); 242 double cb = cos(x); 243 double sb = sin(x); 244 245 double m00, m01, m02, m10, m11, m12, m20, m21, m22; 246 247 m00 = ch * ca; 248 m01 = sh*sb - ch*sa*cb; 249 m02 = ch*sa*sb + sh*cb; 250 m10 = sa; 251 m11 = ca*cb; 252 m12 = -ca*sb; 253 m20 = -sh*ca; 254 m21 = sh*sa*cb + ch*sb; 255 m22 = -sh*sa*sb + ch*cb; 256 257 rotationMatrix.at<double>(0,0) = m00; 258 rotationMatrix.at<double>(0,1) = m01; 259 rotationMatrix.at<double>(0,2) = m02; 260 rotationMatrix.at<double>(1,0) = m10; 261 rotationMatrix.at<double>(1,1) = m11; 262 rotationMatrix.at<double>(1,2) = m12; 263 rotationMatrix.at<double>(2,0) = m20; 264 rotationMatrix.at<double>(2,1) = m21; 265 rotationMatrix.at<double>(2,2) = m22; 266 267 return rotationMatrix; 268 } 269 270 // Converts a given string to an integer 271 int StringToInt ( const std::string &Text ) 272 { 273 std::istringstream ss(Text); 274 int result; 275 return ss >> result ? result : 0; 276 } 277 278 // Converts a given float to a string 279 std::string FloatToString ( float Number ) 280 { 281 std::ostringstream ss; 282 ss << Number; 283 return ss.str(); 284 } 285 286 // Converts a given integer to a string 287 std::string IntToString ( int Number ) 288 { 289 std::ostringstream ss; 290 ss << Number; 291 return ss.str(); 292 } 293
