MRPT  1.9.9
pinhole.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
12 #include <mrpt/img/TCamera.h>
13 #include <mrpt/poses/poses_frwds.h>
14 #include <mrpt/vision/utils.h>
15 
16 namespace mrpt
17 {
18 namespace vision
19 {
20 /** Functions related to pinhole camera models, point projections, etc. \ingroup
21  * mrpt_vision_grp */
22 namespace pinhole
23 {
24 /** \addtogroup mrpt_vision_grp
25  * @{ */
26 
27 /** Project a set of 3D points into a camera at an arbitrary 6D pose using its
28  * calibration matrix (undistorted projection model)
29  * \param in_points_3D [IN] The list of 3D points in world coordinates (meters)
30  * to project.
31  * \param cameraPose [IN] The pose of the camera in the world.
32  * \param intrinsicParams [IN] The 3x3 calibration matrix. See
33  * https://www.mrpt.org/Camera_Parameters
34  * \param projectedPoints [OUT] The list of image coordinates (in pixels) for
35  * the projected points. At output this list is resized to the same number of
36  * input points.
37  * \param accept_points_behind [IN] See the note below.
38  *
39  * \note Points "behind" the camera (which couldn't be physically seen in the
40  * real world) are marked with pixel coordinates (-1,-1) to detect them as
41  * invalid, unless accept_points_behind is true. In that case they'll be
42  * projected normally.
43  *
44  * \sa projectPoints_with_distortion, projectPoint_no_distortion
45  */
47  const std::vector<mrpt::math::TPoint3D>& in_points_3D,
48  const mrpt::poses::CPose3D& cameraPose,
49  const mrpt::math::CMatrixDouble33& intrinsicParams,
50  std::vector<mrpt::img::TPixelCoordf>& projectedPoints,
51  bool accept_points_behind = false);
52 
53 /** Project a single 3D point with global coordinates P into a camera at pose F,
54  *without distortion parameters.
55  * The template argument INVERSE_CAM_POSE is related on how the camera pose
56  *"F" is stored:
57  * - INVERSE_CAM_POSE:false -> The local coordinates of the feature wrt the
58  *camera F are: \f$ P \ominus F \f$
59  * - INVERSE_CAM_POSE:true -> The local coordinates of the feature wrt the
60  *camera F are: \f$ F \oplus P \f$
61  */
62 template <bool INVERSE_CAM_POSE>
64  const mrpt::img::TCamera& cam_params, const mrpt::poses::CPose3D& F,
65  const mrpt::math::TPoint3D& P)
66 {
67  double x, y, z; // wrt cam (local coords)
68  if (INVERSE_CAM_POSE)
69  F.composePoint(P.x, P.y, P.z, x, y, z);
70  else
71  F.inverseComposePoint(P.x, P.y, P.z, x, y, z);
72  ASSERT_(z != 0);
73  // Pinhole model:
75  cam_params.cx() + cam_params.fx() * x / z,
76  cam_params.cy() + cam_params.fy() * y / z);
77 }
78 
79 //! \overload
80 template <typename POINT>
82  const POINT& in_point_wrt_cam, const mrpt::img::TCamera& cam_params,
83  mrpt::img::TPixelCoordf& out_projectedPoints)
84 {
85  ASSERT_(in_point_wrt_cam.z != 0);
86  // Pinhole model:
87  out_projectedPoints.x = cam_params.cx() + cam_params.fx() *
88  in_point_wrt_cam.x /
89  in_point_wrt_cam.z;
90  out_projectedPoints.y = cam_params.cy() + cam_params.fy() *
91  in_point_wrt_cam.y /
92  in_point_wrt_cam.z;
93 }
94 
95 /** Project a set of 3D points into a camera at an arbitrary 6D pose using its
96  * calibration matrix and distortion parameters (radial and tangential
97  * distortions projection model)
98  * \param in_points_3D [IN] The list of 3D points in world coordinates (meters)
99  * to project.
100  * \param cameraPose [IN] The pose of the camera in the world.
101  * \param intrinsicParams [IN] The 3x3 calibration matrix. See
102  * https://www.mrpt.org/Camera_Parameters
103  * \param distortionParams [IN] The 4-length vector with the distortion
104  * parameters [k1 k2 p1 p2]. See https://www.mrpt.org/Camera_Parameters
105  * \param projectedPoints [OUT] The list of image coordinates (in pixels) for
106  * the projected points. At output this list is resized to the same number of
107  * input points.
108  * \param accept_points_behind [IN] See the note below.
109  *
110  * \note Points "behind" the camera (which couldn't be physically seen in the
111  * real world) are marked with pixel coordinates (-1,-1) to detect them as
112  * invalid, unless accept_points_behind is true. In that case they'll be
113  * projected normally.
114  *
115  * \sa projectPoint_with_distortion, projectPoints_no_distortion
116  */
118  const std::vector<mrpt::math::TPoint3D>& in_points_3D,
119  const mrpt::poses::CPose3D& cameraPose,
120  const mrpt::math::CMatrixDouble33& intrinsicParams,
121  const std::vector<double>& distortionParams,
122  std::vector<mrpt::img::TPixelCoordf>& projectedPoints,
123  bool accept_points_behind = false);
124 
125 /** Project one 3D point into a camera using its calibration matrix and
126  * distortion parameters (radial and tangential distortions projection model)
127  * \param in_point_wrt_cam [IN] The 3D point wrt the camera focus, with
128  * +Z=optical axis, +X=righthand in the image plane, +Y=downward in the image
129  * plane.
130  * \param in_cam_params [IN] The camera parameters. See
131  * https://www.mrpt.org/Camera_Parameters
132  * \param out_projectedPoints [OUT] The projected point, in pixel units.
133  * \param accept_points_behind [IN] See the note below.
134  *
135  * \note Points "behind" the camera (which couldn't be physically seen in the
136  * real world) are marked with pixel coordinates (-1,-1) to detect them as
137  * invalid, unless accept_points_behind is true. In that case they'll be
138  * projected normally.
139  *
140  * \sa projectPoints_with_distortion
141  */
143  const mrpt::math::TPoint3D& in_point_wrt_cam,
144  const mrpt::img::TCamera& in_cam_params,
145  mrpt::img::TPixelCoordf& out_projectedPoints,
146  bool accept_points_behind = false);
147 
148 //! \overload
150  const std::vector<mrpt::math::TPoint3D>& P,
151  const mrpt::img::TCamera& params,
152  const mrpt::poses::CPose3DQuat& cameraPose,
153  std::vector<mrpt::img::TPixelCoordf>& pixels,
154  bool accept_points_behind = false);
155 
156 /** Undistort a list of points given by their pixel coordinates, provided the
157  * camera matrix and distortion coefficients.
158  * \param srcDistortedPixels [IN] The pixel coordinates as in the distorted
159  * image.
160  * \param dstUndistortedPixels [OUT] The computed pixel coordinates without
161  * distortion.
162  * \param intrinsicParams [IN] The 3x3 calibration matrix. See
163  * https://www.mrpt.org/Camera_Parameters
164  * \param distortionParams [IN] The 4-length vector with the distortion
165  * parameters [k1 k2 p1 p2]. See https://www.mrpt.org/Camera_Parameters
166  * \sa undistort_point
167  */
168 void undistort_points(
169  const std::vector<mrpt::img::TPixelCoordf>& srcDistortedPixels,
170  std::vector<mrpt::img::TPixelCoordf>& dstUndistortedPixels,
171  const mrpt::math::CMatrixDouble33& intrinsicParams,
172  const std::vector<double>& distortionParams);
173 
174 /** Undistort a list of points given by their pixel coordinates, provided the
175  * camera matrix and distortion coefficients.
176  * \param srcDistortedPixels [IN] The pixel coordinates as in the distorted
177  * image.
178  * \param dstUndistortedPixels [OUT] The computed pixel coordinates without
179  * distortion.
180  * \param cameraModel [IN] The camera parameters.
181  * \sa undistort_point
182  */
183 void undistort_points(
184  const std::vector<mrpt::img::TPixelCoordf>& srcDistortedPixels,
185  std::vector<mrpt::img::TPixelCoordf>& dstUndistortedPixels,
186  const mrpt::img::TCamera& cameraModel);
187 
188 /** Undistort one point given by its pixel coordinates and the camera
189  * parameters.
190  * \sa undistort_points
191  */
192 void undistort_point(
194  const mrpt::img::TCamera& cameraModel);
195 
196 /** @} */ // end of grouping
197 } // namespace pinhole
198 } // namespace vision
199 } // namespace mrpt
void projectPoint_with_distortion(const mrpt::math::TPoint3D &in_point_wrt_cam, const mrpt::img::TCamera &in_cam_params, mrpt::img::TPixelCoordf &out_projectedPoints, bool accept_points_behind=false)
Project one 3D point into a camera using its calibration matrix and distortion parameters (radial and...
Definition: pinhole.cpp:303
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:174
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt) const
Computes the 3D point L such as .
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:176
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:172
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:25
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
GLint GLint GLsizei GLsizei GLsizei GLint GLenum GLenum const GLvoid * pixels
Definition: glext.h:3606
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::img::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...
Definition: pinhole.cpp:51
void undistort_point(const mrpt::img::TPixelCoordf &inPt, mrpt::img::TPixelCoordf &outPt, const mrpt::img::TCamera &cameraModel)
Undistort one point given by its pixel coordinates and the camera parameters.
Definition: pinhole.cpp:204
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:170
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void undistort_points(const std::vector< mrpt::img::TPixelCoordf > &srcDistortedPixels, std::vector< mrpt::img::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...
Definition: pinhole.cpp:133
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::img::TPixelCoordf projectPoint_no_distortion(const mrpt::img::TCamera &cam_params, const mrpt::poses::CPose3D &F, const mrpt::math::TPoint3D &P)
Project a single 3D point with global coordinates P into a camera at pose F, without distortion param...
Definition: pinhole.h:63
GLenum GLint GLint y
Definition: glext.h:3542
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::img::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...
Definition: pinhole.cpp:31
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
Definition: TPoint3D.h:90
GLenum const GLfloat * params
Definition: glext.h:3538



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 836b840ab Mon Nov 18 00:58:29 2019 +0100 at lun nov 18 01:00:14 CET 2019