25 {
26 if (std::isless(point3d[2], 0.f)) {
27 AERROR <<
"The input point (" << point3d
28 << ") should be in front of the camera";
29 }
30
34
40
41 Eigen::Vector2f pt2d_img;
42
43 const Eigen::Vector2f pt_normalized(point3d[0] / point3d[2],
44 point3d[1] / point3d[2]);
45 const float x_n = pt_normalized[0];
46 const float y_n = pt_normalized[1];
47 const float x_mul_x = x_n * x_n;
48 const float y_mul_y = y_n * y_n;
49 const float x_mul_y = x_n * y_n;
50 const float r_squared = x_mul_x + y_mul_y;
51 const float r_to_the_4th = r_squared * r_squared;
52 const float r_to_the_6th = r_squared * r_to_the_4th;
53
54
55 pt2d_img = pt_normalized *
56 (1 + k1 * r_squared + k2 * r_to_the_4th + k3 * r_to_the_6th) /
57 (1 + k4 * r_squared + k5 * r_to_the_4th + k6 * r_to_the_6th);
58
59
60 pt2d_img[0] += 2 * p1 * x_mul_y + p2 * (r_squared + 2 * x_mul_x);
61 pt2d_img[1] += p1 * (r_squared + 2 * y_mul_y) + 2 * p2 * x_mul_y;
62
63
68 pt2d_img[0] = fx * pt2d_img[0] + cx;
69 pt2d_img[1] = fy * pt2d_img[1] + cy;
70
71 return pt2d_img;
72}