| 1269 | |
| 1270 | template <bool Enable, typename std::enable_if<Enable, int>::type> |
| 1271 | bool SimpleRadialCameraModel::ImgFromCamWithJac(const double* params, |
| 1272 | const double& u, |
| 1273 | const double& v, |
| 1274 | const double& w, |
| 1275 | double* x, |
| 1276 | double* y, |
| 1277 | double* J_params, |
| 1278 | double* J_uvw) { |
| 1279 | if (w < std::numeric_limits<double>::epsilon()) { |
| 1280 | return false; |
| 1281 | } |
| 1282 | |
| 1283 | const double f = params[0]; |
| 1284 | const double c1 = params[1]; |
| 1285 | const double c2 = params[2]; |
| 1286 | const double k = params[3]; |
| 1287 | |
| 1288 | const double inv_w = 1.0 / w; |
| 1289 | const double uu = u * inv_w; |
| 1290 | const double vv = v * inv_w; |
| 1291 | |
| 1292 | const double uu2 = uu * uu; |
| 1293 | const double vv2 = vv * vv; |
| 1294 | const double r2 = uu2 + vv2; |
| 1295 | const double k_r2 = k * r2; |
| 1296 | const double alpha = 1.0 + k_r2; |
| 1297 | const double xd = alpha * uu; |
| 1298 | const double yd = alpha * vv; |
| 1299 | |
| 1300 | *x = f * xd + c1; |
| 1301 | *y = f * yd + c2; |
| 1302 | |
| 1303 | if (J_uvw) { |
| 1304 | // J_uvw is a 2x3 matrix (row-major): d(x, y) / d(u, v, w) |
| 1305 | // |
| 1306 | // x = f * alpha * uu + c1, y = f * alpha * vv + c2 |
| 1307 | // where alpha = 1 + k * r2, r2 = uu^2 + vv^2, uu = u/w, vv = v/w |
| 1308 | // |
| 1309 | // Using chain rule: |
| 1310 | // dx/du = f/w * (alpha + 2*k*uu^2) |
| 1311 | // dx/dv = f/w * 2*k*uu*vv |
| 1312 | // dx/dw = -f*uu/w * (1 + 3*k*r2) |
| 1313 | // dy/du = f/w * 2*k*uu*vv |
| 1314 | // dy/dv = f/w * (alpha + 2*k*vv^2) |
| 1315 | // dy/dw = -f*vv/w * (1 + 3*k*r2) |
| 1316 | |
| 1317 | const double two_k = 2.0 * k; |
| 1318 | const double f_inv_w = f * inv_w; |
| 1319 | const double beta = 1.0 + 3.0 * k_r2; |
| 1320 | const double two_k_uu_vv = two_k * uu * vv; |
| 1321 | |
| 1322 | J_uvw[0] = f_inv_w * (alpha + two_k * uu2); |
| 1323 | J_uvw[1] = f_inv_w * two_k_uu_vv; |
| 1324 | J_uvw[2] = -f_inv_w * uu * beta; |
| 1325 | J_uvw[3] = f_inv_w * two_k_uu_vv; |
| 1326 | J_uvw[4] = f_inv_w * (alpha + two_k * vv2); |
| 1327 | J_uvw[5] = -f_inv_w * vv * beta; |
| 1328 | } |
nothing calls this directly
no outgoing calls
no test coverage detected