16 #include <mrpt/3rdparty/do_opencv_includes.h>
32 const std::vector<mrpt::math::TPoint3D>& in_points_3D,
35 std::vector<TPixelCoordf>& projectedPoints,
bool accept_points_behind)
40 static const std::vector<double> distortion_dummy(4, 0);
43 in_points_3D, cameraPose, intrinsicParams, distortion_dummy,
44 projectedPoints, accept_points_behind);
52 const std::vector<mrpt::math::TPoint3D>& in_points_3D,
55 const std::vector<double>& distortionParams,
56 std::vector<mrpt::img::TPixelCoordf>& projectedPoints,
57 bool accept_points_behind)
65 const size_t N = in_points_3D.size();
66 projectedPoints.resize(N);
70 vector<CvPoint3D64f> objPoints(N);
73 for (
size_t i = 0; i < N; i++)
75 in_points_3D[i].x, in_points_3D[i].y, in_points_3D[i].z,
76 objPoints[i].x, objPoints[i].y, objPoints[i].z);
79 static double rotation_matrix[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
80 static double translation_vector[] = {0, 0, 0};
86 std::vector<double> proj_matrix(9);
87 proj_matrix[0] = intrinsicParams(0, 0);
88 proj_matrix[4] = intrinsicParams(1, 1);
89 proj_matrix[2] = intrinsicParams(0, 2);
90 proj_matrix[5] = intrinsicParams(1, 2);
93 cv::Mat object_points = cv::Mat(N, 1, CV_64FC3, &objPoints[0]);
96 cv::Rodrigues(cv::Mat(3, 3, CV_64FC1, rotation_matrix), rotvec);
98 cv::Mat _translation_vector = cv::Mat(3, 1, CV_64FC1, translation_vector);
99 cv::Mat camera_matrix = cv::Mat(3, 3, CV_64FC1, &proj_matrix[0]);
100 cv::Mat dist_coeffs = cv::Mat(
101 distortionParams.size(), 1, CV_64FC1,
102 const_cast<double*
>(&distortionParams[0]));
104 vector<cv::Point2d> image_points;
107 object_points, rotvec, _translation_vector, camera_matrix, dist_coeffs,
110 for (
size_t i = 0; i < N; i++)
112 if (accept_points_behind || objPoints[i].z > 0)
114 projectedPoints[i].x =
d2f(image_points[i].x);
115 projectedPoints[i].y =
d2f(image_points[i].y);
119 projectedPoints[i].x = -1;
120 projectedPoints[i].y = -1;
125 THROW_EXCEPTION(
"Function not available: MRPT was compiled without OpenCV");
134 const std::vector<mrpt::img::TPixelCoordf>& in_dist_pixels,
135 std::vector<mrpt::img::TPixelCoordf>& out_pixels,
141 for (
size_t i = 0; i < cam.
dist.size(); i++) cam.
dist[i] = Dk[i];
146 const std::vector<mrpt::img::TPixelCoordf>& in_dist_pixels,
147 std::vector<mrpt::img::TPixelCoordf>& out_pixels,
156 const size_t n = in_dist_pixels.size();
157 out_pixels.resize(n);
159 const double fx = cameraModel.
fx();
160 const double fy = cameraModel.
fy();
161 const double ifx = 1. / fx;
162 const double ify = 1. / fy;
163 const double cx = cameraModel.
cx();
164 const double cy = cameraModel.
cy();
166 for (
size_t i = 0; i < n; i++)
168 double x = in_dist_pixels[i].x;
169 double y = in_dist_pixels[i].y;
171 double x0 = x = (x - cx) * ifx;
172 double y0 = y = (y - cy) * ify;
175 for (
unsigned int j = 0; j < 5; j++)
177 double r2 = x * x + y * y;
180 (1 + ((cameraModel.
dist[4] * r2 + cameraModel.
dist[1]) * r2 +
181 cameraModel.
dist[0]) *
183 double deltaX = 2 * cameraModel.
dist[2] * x * y +
184 cameraModel.
dist[3] * (r2 + 2 * x * x);
185 double deltaY = cameraModel.
dist[2] * (r2 + 2 * y * y) +
186 2 * cameraModel.
dist[3] * x * y;
187 x = (x0 - deltaX) * icdist;
188 y = (y0 - deltaY) * icdist;
192 out_pixels[i].x =
d2f(x * fx + cx);
193 out_pixels[i].y =
d2f(y * fy + cy);
214 const double fx = cameraModel.
fx();
215 const double fy = cameraModel.
fy();
216 const double ifx = 1. / fx;
217 const double ify = 1. / fy;
218 const double cx = cameraModel.
cx();
219 const double cy = cameraModel.
cy();
224 double x0 = x = (x - cx) * ifx;
225 double y0 = y = (y - cy) * ify;
228 for (
unsigned int j = 0; j < 5; j++)
230 double r2 = x * x + y * y;
232 1. / (1 + ((cameraModel.
dist[4] * r2 + cameraModel.
dist[1]) * r2 +
233 cameraModel.
dist[0]) *
235 double deltaX = 2 * cameraModel.
dist[2] * x * y +
236 cameraModel.
dist[3] * (r2 + 2 * x * x);
237 double deltaY = cameraModel.
dist[2] * (r2 + 2 * y * y) +
238 2 * cameraModel.
dist[3] * x * y;
239 x = (x0 - deltaX) * icdist;
240 y = (y0 - deltaY) * icdist;
244 outPt.
x =
d2f(x * fx + cx);
245 outPt.
y =
d2f(y * fy + cy);
251 const std::vector<mrpt::math::TPoint3D>& P,
254 std::vector<mrpt::img::TPixelCoordf>& pixels,
bool accept_points_behind)
258 pixels.resize(P.size());
259 std::vector<mrpt::math::TPoint3D>::const_iterator itPoints;
260 std::vector<mrpt::img::TPixelCoordf>::iterator itPixels;
262 for (itPoints = P.begin(), itPixels = pixels.begin(); itPoints != P.end();
263 ++itPoints, ++itPixels, ++k)
268 itPoints->x, itPoints->y, itPoints->z, nP.
x, nP.
y, nP.
z);
271 const double x = nP.
x / nP.
z;
272 const double y = nP.
y / nP.
z;
276 const double r4 =
square(r2);
277 const double r6 = r2 * r4;
280 const double B = 2 * x * y;
281 if (
A > 0 && (accept_points_behind || nP.
z > 0))
310 const double x = P.
x / P.
z;
311 const double y = P.
y / P.
z;
315 const double r4 =
square(r2);
316 const double r6 = r2 * r4;
322 2 *
params.dist[2] * x * y +
328 2 *
params.dist[3] * x * y +