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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020