OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
PinholeCamera.hpp
Go to the documentation of this file.
1 /*********************************************************************************
2  * OKVIS - Open Keyframe-based Visual-Inertial SLAM
3  * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
14  * its contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Created on: Jan 28, 2015
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  * Modified: Andreas Forster (an.forster@gmail.com)
32  *********************************************************************************/
33 
41 #ifndef INCLUDE_OKVIS_CAMERAS_PINHOLECAMERA_HPP_
42 #define INCLUDE_OKVIS_CAMERAS_PINHOLECAMERA_HPP_
43 
44 #include <vector>
45 #include <memory>
46 #include <stdint.h>
47 #include <Eigen/Core>
48 #pragma GCC diagnostic push
49 #pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
50 #include <opencv2/core/core.hpp> // Code that causes warning goes here
51 #pragma GCC diagnostic pop
54 
56 namespace okvis {
58 namespace cameras {
59 
63 template<class DISTORTION_T>
64 class PinholeCamera : public CameraBase
65 {
66  public:
67  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
68  typedef DISTORTION_T distortion_t;
69 
80  double focalLengthV, double imageCenterU, double imageCenterV,
81  const distortion_t & distortion, uint64_t id=-1);
82 
85  {
86  }
87 
88  static const int NumProjectionIntrinsics = 4;
90  + distortion_t::NumDistortionIntrinsics;
91 
94  double focalLengthU() const
95  {
96  return fu_;
97  }
98 
101  double focalLengthV() const
102  {
103  return fv_;
104  }
105 
108  double imageCenterU() const
109  {
110  return cu_;
111  }
112 
115  double imageCenterV() const
116  {
117  return cv_;
118  }
119 
122  inline void getIntrinsics(Eigen::VectorXd & intrinsics) const ;
123 
126  inline bool setIntrinsics(const Eigen::VectorXd & intrinsics);
127 
130  inline int noIntrinsicsParameters() const
131  {
132  return NumIntrinsics;
133  }
134 
138 
146  const Eigen::Vector3d & point, Eigen::Vector2d * imagePoint) const;
147 
157  const Eigen::Vector3d & point, Eigen::Vector2d * imagePoint,
158  Eigen::Matrix<double, 2, 3> * pointJacobian,
159  Eigen::Matrix2Xd * intrinsicsJacobian = NULL) const;
160 
171  const Eigen::Vector3d & point, const Eigen::VectorXd & parameters,
172  Eigen::Vector2d * imagePoint, Eigen::Matrix<double, 2, 3> * pointJacobian,
173  Eigen::Matrix2Xd * intrinsicsJacobian = NULL) const;
174 
181  inline void projectBatch(
182  const Eigen::Matrix3Xd & points, Eigen::Matrix2Xd * imagePoints,
183  std::vector<CameraBase::ProjectionStatus> * stati) const;
184 
192  const Eigen::Vector4d & point, Eigen::Vector2d * imagePoint) const;
193 
203  const Eigen::Vector4d & point, Eigen::Vector2d * imagePoint,
204  Eigen::Matrix<double, 2, 4> * pointJacobian,
205  Eigen::Matrix2Xd * intrinsicsJacobian = NULL) const;
206 
217  const Eigen::Vector4d & point, const Eigen::VectorXd & parameters,
218  Eigen::Vector2d * imagePoint,
219  Eigen::Matrix<double, 2, 4> * pointJacobian = NULL,
220  Eigen::Matrix2Xd * intrinsicsJacobian = NULL) const;
221 
228  inline void projectHomogeneousBatch(
229  const Eigen::Matrix4Xd & points, Eigen::Matrix2Xd * imagePoints,
230  std::vector<CameraBase::ProjectionStatus> * stati) const;
232 
236 
241  inline bool backProject(const Eigen::Vector2d & imagePoint,
242  Eigen::Vector3d * direction) const;
243 
249  inline bool backProject(const Eigen::Vector2d & imagePoint,
250  Eigen::Vector3d * direction,
251  Eigen::Matrix<double, 3, 2> * pointJacobian) const;
252 
257  inline bool backProjectBatch(const Eigen::Matrix2Xd & imagePoints,
258  Eigen::Matrix3Xd * directions,
259  std::vector<bool> * success) const;
260 
265  inline bool backProjectHomogeneous(const Eigen::Vector2d & imagePoint,
266  Eigen::Vector4d * direction) const;
267 
273  inline bool backProjectHomogeneous(
274  const Eigen::Vector2d & imagePoint, Eigen::Vector4d * direction,
275  Eigen::Matrix<double, 4, 2> * pointJacobian) const;
276 
281  inline bool backProjectHomogeneousBatch(const Eigen::Matrix2Xd & imagePoints,
282  Eigen::Matrix4Xd * directions,
283  std::vector<bool> * success) const;
285 
287  static std::shared_ptr<CameraBase> createTestObject()
288  {
289  return std::shared_ptr<CameraBase>(new PinholeCamera(752, 480, 350, 360, 378, 238,
290  distortion_t::testObject()));
291  }
294  {
295  return PinholeCamera(752, 480, 350, 360, 378, 238,
296  distortion_t::testObject());
297  }
298 
300  std::string type() const
301  {
302  return "PinholeCamera<" + distortion_.type() + ">";
303  }
304 
306  const std::string distortionType() const
307  {
308  return distortion_.type();
309  }
310 
311  protected:
312 
314  PinholeCamera() = delete;
315 
317 
318  Eigen::Matrix<double, NumIntrinsics, 1> intrinsics_;
319  double fu_;
320  double fv_;
321  double cu_;
322  double cv_;
323  double one_over_fu_;
324  double one_over_fv_;
325  double fu_over_fv_;
326 
327 };
328 
329 } // namespace cameras
330 } // namespace okvis
331 
333 
334 #endif /* INCLUDE_OKVIS_CAMERAS_PINHOLECAMERA_HPP_ */
static const int NumProjectionIntrinsics
optimisable projection intrinsics
Definition: PinholeCamera.hpp:88
ProjectionStatus
Definition: CameraBase.hpp:67
PinholeCamera()=delete
No default constructor.
bool backProjectHomogeneousBatch(const Eigen::Matrix2Xd &imagePoints, Eigen::Matrix4Xd *directions, std::vector< bool > *success) const
Back-project 2d image points into homogeneous points (direction vectors).
Definition: PinholeCamera.hpp:528
double fu_
focalLengthU
Definition: PinholeCamera.hpp:319
Header implementation file for the PinholeCamera class.
double one_over_fu_
1.0 / fu_
Definition: PinholeCamera.hpp:323
double fv_
focalLengthV
Definition: PinholeCamera.hpp:320
double fu_over_fv_
fu_ / fv_
Definition: PinholeCamera.hpp:325
static const int NumIntrinsics
total number of intrinsics
Definition: PinholeCamera.hpp:89
CameraBase::ProjectionStatus project(const Eigen::Vector3d &point, Eigen::Vector2d *imagePoint) const
Projects a Euclidean point to a 2d image point (projection). Uses projection including distortion mod...
Definition: PinholeCamera.hpp:109
distortion_t distortion_
the distortion to be used
Definition: PinholeCamera.hpp:316
Eigen::Matrix< double, NumIntrinsics, 1 > intrinsics_
summary of all intrinsics parameters
Definition: PinholeCamera.hpp:318
uint32_t imageHeight() const
The height of the image in pixels.
Definition: CameraBase.hpp:143
bool backProjectHomogeneous(const Eigen::Vector2d &imagePoint, Eigen::Vector4d *direction) const
Back-project a 2d image point into homogeneous point (direction vector).
Definition: PinholeCamera.hpp:500
CameraBase::ProjectionStatus projectWithExternalParameters(const Eigen::Vector3d &point, const Eigen::VectorXd &parameters, Eigen::Vector2d *imagePoint, Eigen::Matrix< double, 2, 3 > *pointJacobian, Eigen::Matrix2Xd *intrinsicsJacobian=NULL) const
Projects a Euclidean point to a 2d image point (projection). Uses projection including distortion mod...
Definition: PinholeCamera.hpp:230
Definition: PinholeCamera.hpp:64
bool backProjectBatch(const Eigen::Matrix2Xd &imagePoints, Eigen::Matrix3Xd *directions, std::vector< bool > *success) const
Back-project 2d image points into Euclidean space (direction vectors).
Definition: PinholeCamera.hpp:481
double imageCenterU() const
Get the image centre along the u-dimension.
Definition: PinholeCamera.hpp:108
std::string type() const
Obtain the projection type.
Definition: PinholeCamera.hpp:300
CameraBase::ProjectionStatus projectHomogeneousWithExternalParameters(const Eigen::Vector4d &point, const Eigen::VectorXd &parameters, Eigen::Vector2d *imagePoint, Eigen::Matrix< double, 2, 4 > *pointJacobian=NULL, Eigen::Matrix2Xd *intrinsicsJacobian=NULL) const
Projects a point in homogenous coordinates to a 2d image point (projection). Uses projection includin...
Definition: PinholeCamera.hpp:382
bool backProject(const Eigen::Vector2d &imagePoint, Eigen::Vector3d *direction) const
Back-project a 2d image point into Euclidean space (direction vector).
Definition: PinholeCamera.hpp:426
double cu_
imageCenterU
Definition: PinholeCamera.hpp:321
Header file for the DistortionBase class.
void projectBatch(const Eigen::Matrix3Xd &points, Eigen::Matrix2Xd *imagePoints, std::vector< CameraBase::ProjectionStatus > *stati) const
Projects Euclidean points to 2d image points (projection) in a batch. Uses projection including disto...
Definition: PinholeCamera.hpp:328
uint32_t imageWidth() const
The width of the image in pixels.
Definition: CameraBase.hpp:138
int noIntrinsicsParameters() const
Get the total number of intrinsics.
Definition: PinholeCamera.hpp:130
bool setIntrinsics(const Eigen::VectorXd &intrinsics)
overwrite all intrinsics - use with caution !
Definition: PinholeCamera.hpp:74
double focalLengthU() const
Get the focal length along the u-dimension.
Definition: PinholeCamera.hpp:94
static PinholeCamera testObject()
get a test instance
Definition: PinholeCamera.hpp:293
Header file for the CameraBase class.
CameraBase::ProjectionStatus projectHomogeneous(const Eigen::Vector4d &point, Eigen::Vector2d *imagePoint) const
Projects a point in homogenous coordinates to a 2d image point (projection). Uses projection includin...
Definition: PinholeCamera.hpp:345
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef DISTORTION_T distortion_t
Makes the distortion type accessible.
Definition: PinholeCamera.hpp:68
~PinholeCamera()
Destructor.
Definition: PinholeCamera.hpp:84
void projectHomogeneousBatch(const Eigen::Matrix4Xd &points, Eigen::Matrix2Xd *imagePoints, std::vector< CameraBase::ProjectionStatus > *stati) const
Projects points in homogenous coordinates to 2d image points (projection) in a batch. Uses projection including distortion models.
Definition: PinholeCamera.hpp:406
double one_over_fv_
1.0 / fv_
Definition: PinholeCamera.hpp:324
Base class for all camera models.
Definition: CameraBase.hpp:60
const std::string distortionType() const
Obtain the projection type.
Definition: PinholeCamera.hpp:306
void getIntrinsics(Eigen::VectorXd &intrinsics) const
Get the intrinsics as a concatenated vector.
Definition: PinholeCamera.hpp:94
double focalLengthV() const
Get the focal length along the v-dimension.
Definition: PinholeCamera.hpp:101
double imageCenterV() const
Get the focal image centre along the v-dimension.
Definition: PinholeCamera.hpp:115
static std::shared_ptr< CameraBase > createTestObject()
get a test instance
Definition: PinholeCamera.hpp:287
double cv_
imageCenterV
Definition: PinholeCamera.hpp:322