15 #include <mrpt/otherlibs/do_opencv_includes.h> 31 const std::vector<mrpt::math::TPoint3D>& in_points_3D,
34 std::vector<TPixelCoordf>& projectedPoints,
bool accept_points_behind)
39 static const std::vector<double> distortion_dummy(4, 0);
42 in_points_3D, cameraPose, intrinsicParams, distortion_dummy,
43 projectedPoints, accept_points_behind);
51 const std::vector<mrpt::math::TPoint3D>& in_points_3D,
54 const std::vector<double>& distortionParams,
55 std::vector<mrpt::utils::TPixelCoordf>& projectedPoints,
56 bool accept_points_behind)
63 ASSERT_(distortionParams.size() == 4 || distortionParams.size() == 5);
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};
87 proj_matrix[0] = intrinsicParams.get_unsafe(0, 0);
88 proj_matrix[4] = intrinsicParams.get_unsafe(1, 1);
89 proj_matrix[2] = intrinsicParams.get_unsafe(0, 2);
90 proj_matrix[5] = intrinsicParams.get_unsafe(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 =
101 cv::Mat(5, 1, CV_64FC1, const_cast<double*>(&distortionParams[0]));
103 vector<cv::Point2d> image_points;
106 object_points, rotvec, _translation_vector, camera_matrix, dist_coeffs,
109 for (
size_t i = 0; i < N; i++)
111 if (accept_points_behind || objPoints[i].
z > 0)
113 projectedPoints[i].x = image_points[i].x;
114 projectedPoints[i].y = image_points[i].y;
118 projectedPoints[i].x = -1;
119 projectedPoints[i].y = -1;
124 THROW_EXCEPTION(
"Function not available: MRPT was compiled without OpenCV")
133 const std::vector<mrpt::utils::TPixelCoordf>& in_dist_pixels,
134 std::vector<mrpt::utils::TPixelCoordf>& out_pixels,
140 for (
size_t i = 0; i < cam.
dist.static_size; i++) cam.
dist[i] = Dk[i];
145 const std::vector<mrpt::utils::TPixelCoordf>& in_dist_pixels,
146 std::vector<mrpt::utils::TPixelCoordf>& out_pixels,
155 const size_t n = in_dist_pixels.size();
156 out_pixels.resize(
n);
158 const double fx = cameraModel.
fx();
159 const double fy = cameraModel.
fy();
160 const double ifx = 1. / fx;
161 const double ify = 1. / fy;
162 const double cx = cameraModel.
cx();
163 const double cy = cameraModel.
cy();
165 for (
size_t i = 0; i <
n; i++)
167 double x = in_dist_pixels[i].x;
168 double y = in_dist_pixels[i].y;
170 double x0 =
x = (
x - cx) * ifx;
171 double y0 =
y = (
y - cy) * ify;
174 for (
unsigned int j = 0; j < 5; j++)
176 double r2 =
x *
x +
y *
y;
179 ((cameraModel.
dist[4] * r2 + cameraModel.
dist[1]) * r2 +
180 cameraModel.
dist[0]) *
182 double deltaX = 2 * cameraModel.
dist[2] *
x *
y +
183 cameraModel.
dist[3] * (r2 + 2 *
x *
x);
184 double deltaY = cameraModel.
dist[2] * (r2 + 2 *
y *
y) +
185 2 * cameraModel.
dist[3] *
x *
y;
186 x = (x0 - deltaX) * icdist;
187 y = (y0 - deltaY) * icdist;
191 out_pixels[i].x =
x * fx + cx;
192 out_pixels[i].y =
y * fy + cy;
213 const double fx = cameraModel.
fx();
214 const double fy = cameraModel.
fy();
215 const double ifx = 1. / fx;
216 const double ify = 1. / fy;
217 const double cx = cameraModel.
cx();
218 const double cy = cameraModel.
cy();
223 double x0 =
x = (
x - cx) * ifx;
224 double y0 =
y = (
y - cy) * ify;
227 for (
unsigned int j = 0; j < 5; j++)
229 double r2 =
x *
x +
y *
y;
232 ((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 =
x * fx + cx;
245 outPt.
y =
y * fy + cy;
251 const std::vector<mrpt::math::TPoint3D>& P,
254 std::vector<mrpt::utils::TPixelCoordf>&
pixels,
bool accept_points_behind)
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))
283 itPixels->x =
params.cx() +
286 itPixels->y =
params.cy() +
309 const double x = P.
x / P.
z;
310 const double y = P.
y / P.
z;
314 const double r4 =
square(r2);
315 const double r6 = r2 * r4;
A pair (x,y) of pixel coordinates (subpixel resolution).
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point L such as .
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
#define THROW_EXCEPTION(msg)
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
void projectPoints_with_distortion(const std::vector< mrpt::math::TPoint3D > &in_points_3D, const mrpt::poses::CPose3D &cameraPose, const mrpt::math::CMatrixDouble33 &intrinsicParams, const std::vector< double > &distortionParams, std::vector< mrpt::utils::TPixelCoordf > &projectedPoints, bool accept_points_behind=false)
Project a set of 3D points into a camera at an arbitrary 6D pose using its calibration matrix and dis...
const Scalar * const_iterator
T square(const T x)
Inline function for the square of a number.
double fx() const
Get the value of the focal length x-value (in pixels).
double fy() const
Get the value of the focal length y-value (in pixels).
This base provides a set of functions for maths stuff.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Functions related to pinhole camera models, point projections, etc.
This namespace contains representation of robot actions and observations.
Classes for computer vision, detectors, features, etc.
double x
X,Y,Z coordinates.
void projectPoints_no_distortion(const std::vector< mrpt::math::TPoint3D > &in_points_3D, const mrpt::poses::CPose3D &cameraPose, const mrpt::math::CMatrixDouble33 &intrinsicParams, std::vector< mrpt::utils::TPixelCoordf > &projectedPoints, bool accept_points_behind=false)
Project a set of 3D points into a camera at an arbitrary 6D pose using its calibration matrix (undist...
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
GLint GLint GLsizei GLsizei GLsizei GLint GLenum GLenum const GLvoid * pixels
double cx() const
Get the value of the principal point x-coordinate (in pixels).
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void projectPoint_with_distortion(const mrpt::math::TPoint3D &in_point_wrt_cam, const mrpt::utils::TCamera &in_cam_params, mrpt::utils::TPixelCoordf &out_projectedPoints, bool accept_points_behind=false)
Project one 3D point into a camera using its calibration matrix and distortion parameters (radial and...
void undistort_point(const mrpt::utils::TPixelCoordf &inPt, mrpt::utils::TPixelCoordf &outPt, const mrpt::utils::TCamera &cameraModel)
Undistort one point given by its pixel coordinates and the camera parameters.
mrpt::math::CArrayDouble< 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
void undistort_points(const std::vector< mrpt::utils::TPixelCoordf > &srcDistortedPixels, std::vector< mrpt::utils::TPixelCoordf > &dstUndistortedPixels, const mrpt::math::CMatrixDouble33 &intrinsicParams, const std::vector< double > &distortionParams)
Undistort a list of points given by their pixel coordinates, provided the camera matrix and distortio...
GLenum const GLfloat * params
Structure to hold the parameters of a pinhole camera model.
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .