| 189 | */ |
| 190 | template<typename CvPointType> |
| 191 | static std::vector<CvPointType> cameraToScreen(const std::vector<CvPointType>& points, |
| 192 | const cv::Mat& cameraMatrix, |
| 193 | const cv::Mat& distCoeffs) |
| 194 | { |
| 195 | // We operate with CV_64F matrices internally to avoid precision loss |
| 196 | cv::Mat cm_64f; // camera matrix, CV_64F |
| 197 | cv::Mat dc_64f; // distortion coefficients, CV_64F |
| 198 | cameraMatrix.convertTo(cm_64f, CV_64F); |
| 199 | distCoeffs.convertTo(dc_64f, CV_64F); |
| 200 | |
| 201 | // Make sure distortion vector has a size of (N, 1) |
| 202 | if (dc_64f.rows == 1) |
| 203 | { |
| 204 | dc_64f = dc_64f.t(); |
| 205 | } |
| 206 | |
| 207 | // We will always use 12 distortion coefficients, |
| 208 | // and we can safely pad missing ones with zeroes |
| 209 | dc_64f.resize(12, 0.0); |
| 210 | |
| 211 | std::vector<CvPointType> result; |
| 212 | result.reserve(points.size()); |
| 213 | |
| 214 | for(const auto& point : points) |
| 215 | { |
| 216 | // Apply perspective projection, preserving initial Z coordinate |
| 217 | // Always use double-precision |
| 218 | cv::Point3d camPoint{ |
| 219 | point.x / point.z, |
| 220 | point.y / point.z, |
| 221 | point.z |
| 222 | }; |
| 223 | |
| 224 | // Apply distortion |
| 225 | // Note that we do not consider tilted sensor distortion |
| 226 | // r^2 - distance from the image center squared |
| 227 | double r2 = camPoint.x * camPoint.x + camPoint.y * camPoint.y; |
| 228 | // r^4 - same, but to the 4th power |
| 229 | double r4 = r2 * r2; |
| 230 | // r^6 - same, but to the 6th power |
| 231 | double r6 = r4 * r2; |
| 232 | // tg1 - first tangential shift factor (2 * x * y) |
| 233 | double tg1 = 2 * camPoint.x * camPoint.y; |
| 234 | // tg2 - second tangential shift factor (r^2 + 2 * x^2) |
| 235 | double tg2 = r2 + 2 * camPoint.x * camPoint.x; |
| 236 | // tg3 - third tangential shift factor (r^2 + 2 * y^2) |
| 237 | double tg3 = r2 + 2 * camPoint.y * camPoint.y; |
| 238 | // polynomial distortion factor (numerator) |
| 239 | double pndist = 1 + dc_64f.at<double>(0) * r2 + dc_64f.at<double>(1) * r4 + dc_64f.at<double>(4) * r6; |
| 240 | // polynomial distortion factror (denominator) |
| 241 | double pddist = 1.0 / (1 + dc_64f.at<double>(5) * r2 + dc_64f.at<double>(6) * r4 + dc_64f.at<double>(7) * r6); |
| 242 | // Distorted point coordinates (always double-precision) |
| 243 | cv::Point3d distortedPoint{ |
| 244 | camPoint.x * pndist * pddist + dc_64f.at<double>(2) * tg1 + dc_64f.at<double>(3) * tg2 + dc_64f.at<double>(8) * r2 + dc_64f.at<double>(9) * r4, |
| 245 | camPoint.y * pndist * pddist + dc_64f.at<double>(2) * tg3 + dc_64f.at<double>(3) * tg1 + dc_64f.at<double>(10) * r2 + dc_64f.at<double>(11) * r4, |
| 246 | camPoint.z |
| 247 | }; |
| 248 | |