OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
CameraBase.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  *********************************************************************************/
32 
40 #ifndef INCLUDE_OKVIS_CAMERAS_CAMERABASE_HPP_
41 #define INCLUDE_OKVIS_CAMERAS_CAMERABASE_HPP_
42 
43 #include <vector>
44 #include <memory>
45 #include <stdint.h>
46 #include <Eigen/Core>
47 #pragma GCC diagnostic push
48 #pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
49 #include <opencv2/core/core.hpp> // Code that causes warning goes here
50 #pragma GCC diagnostic pop
52 
54 namespace okvis {
56 namespace cameras {
57 
61 {
62  public:
63  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64 
67  enum class ProjectionStatus
68  {
69  Successful,
71  Masked,
72  Behind,
73  Invalid
74  };
75 
77  inline CameraBase()
78  : imageWidth_(0),
79  imageHeight_(0),
80  id_(0)
81  {
82  }
83 
85  inline CameraBase(int imageWidth, int imageHeight, uint64_t id = 0)
86  : imageWidth_(imageWidth),
87  imageHeight_(imageHeight),
88  id_(id)
89  {
90  }
91 
93  inline virtual ~CameraBase()
94  {
95  }
96 
100 
106  inline bool setMask(const cv::Mat & mask);
107 
109  inline bool hasMask() const;
110 
112  inline bool removeMask();
113 
115  inline const cv::Mat & mask() const;
116 
118 
122 
124  inline void setId(uint64_t id)
125  {
126  id_ = id;
127  }
128 
130  inline uint64_t id() const
131  {
132  return id_;
133  }
134 
136 
138  inline uint32_t imageWidth() const
139  {
140  return imageWidth_;
141  }
143  inline uint32_t imageHeight() const
144  {
145  return imageHeight_;
146  }
147 
149  virtual void getIntrinsics(Eigen::VectorXd & intrinsics) const = 0;
150 
152  virtual bool setIntrinsics(const Eigen::VectorXd & intrinsics) = 0;
153 
157 
164  virtual ProjectionStatus project(const Eigen::Vector3d & point,
165  Eigen::Vector2d * imagePoint) const = 0;
166 
175  virtual ProjectionStatus project(
176  const Eigen::Vector3d & point, Eigen::Vector2d * imagePoint,
177  Eigen::Matrix<double, 2, 3> * pointJacobian,
178  Eigen::Matrix2Xd * intrinsicsJacobian = NULL) const = 0;
179 
190  const Eigen::Vector3d & point, const Eigen::VectorXd & parameters,
191  Eigen::Vector2d * imagePoint, Eigen::Matrix<double, 2, 3> * pointJacobian = NULL,
192  Eigen::Matrix2Xd * intrinsicsJacobian = NULL) const = 0;
193 
200  virtual void projectBatch(const Eigen::Matrix3Xd & points,
201  Eigen::Matrix2Xd * imagePoints,
202  std::vector<ProjectionStatus> * stati) const = 0;
203 
211  const Eigen::Vector4d & point, Eigen::Vector2d * imagePoint) const = 0;
212 
222  const Eigen::Vector4d & point, Eigen::Vector2d * imagePoint,
223  Eigen::Matrix<double, 2, 4> * pointJacobian,
224  Eigen::Matrix2Xd * intrinsicsJacobian = NULL) const = 0;
225 
236  const Eigen::Vector4d & point, const Eigen::VectorXd & parameters,
237  Eigen::Vector2d * imagePoint,
238  Eigen::Matrix<double, 2, 4> * pointJacobian = NULL,
239  Eigen::Matrix2Xd * intrinsicsJacobian = NULL) const = 0;
240 
247  virtual void projectHomogeneousBatch(
248  const Eigen::Matrix4Xd & points, Eigen::Matrix2Xd * imagePoints,
249  std::vector<ProjectionStatus> * stati) const = 0;
251 
255 
260  virtual bool backProject(const Eigen::Vector2d & imagePoint,
261  Eigen::Vector3d * direction) const = 0;
262 
268  virtual bool backProject(
269  const Eigen::Vector2d & imagePoint, Eigen::Vector3d * direction,
270  Eigen::Matrix<double, 3, 2> * pointJacobian) const = 0;
271 
276  virtual bool backProjectBatch(const Eigen::Matrix2Xd & imagePoints,
277  Eigen::Matrix3Xd * directions,
278  std::vector<bool> * success) const = 0;
279 
284  virtual bool backProjectHomogeneous(const Eigen::Vector2d & imagePoint,
285  Eigen::Vector4d * direction) const = 0;
286 
292  virtual bool backProjectHomogeneous(
293  const Eigen::Vector2d & imagePoint, Eigen::Vector4d * direction,
294  Eigen::Matrix<double, 4, 2> * pointJacobian) const = 0;
295 
300  virtual bool backProjectHomogeneousBatch(
301  const Eigen::Matrix2Xd & imagePoints, Eigen::Matrix4Xd * directions,
302  std::vector<bool> * success) const = 0;
304 
308 
311  virtual Eigen::Vector2d createRandomImagePoint() const;
312 
317  virtual Eigen::Vector3d createRandomVisiblePoint(double minDist = 0.0,
318  double maxDist = 10.0) const;
319 
324  virtual Eigen::Vector4d createRandomVisibleHomogeneousPoint(double minDist =
325  0.0,
326  double maxDist =
327  10.0) const;
329 
331  virtual int noIntrinsicsParameters() const = 0;
332 
334  virtual std::string type() const = 0;
335 
337  virtual const std::string distortionType() const = 0;
338 
339  protected:
340 
342  inline bool isMasked(const Eigen::Vector2d& imagePoint) const;
343 
345  inline bool isInImage(const Eigen::Vector2d& imagePoint) const;
346 
347  cv::Mat mask_;
348 
351 
352  uint64_t id_;
353 
354 };
355 
356 } // namespace cameras
357 } // namespace drltools
358 
360 
361 #endif /* INCLUDE_OKVIS_CAMERAS_CAMERABASE_HPP_ */
virtual const std::string distortionType() const =0
Obtain the projection type.
uint64_t id() const
Obtain the Id.
Definition: CameraBase.hpp:130
virtual ProjectionStatus projectWithExternalParameters(const Eigen::Vector3d &point, const Eigen::VectorXd &parameters, Eigen::Vector2d *imagePoint, Eigen::Matrix< double, 2, 3 > *pointJacobian=NULL, Eigen::Matrix2Xd *intrinsicsJacobian=NULL) const =0
Projects a Euclidean point to a 2d image point (projection). Uses projection including distortion mod...
int imageHeight_
image height in pixels
Definition: CameraBase.hpp:350
Indicates what happened when applying any of the project functions.
bool removeMask()
stop masking
Definition: CameraBase.hpp:65
virtual std::string type() const =0
Obtain the type.
bool isInImage(const Eigen::Vector2d &imagePoint) const
Check if the keypoint is in the image.
Definition: CameraBase.hpp:95
virtual Eigen::Vector4d createRandomVisibleHomogeneousPoint(double minDist=0.0, double maxDist=10.0) const
Creates a random visible point in homogeneous coordinates.
Definition: CameraBase.cpp:77
int imageWidth_
image width in pixels
Definition: CameraBase.hpp:349
virtual void projectHomogeneousBatch(const Eigen::Matrix4Xd &points, Eigen::Matrix2Xd *imagePoints, std::vector< ProjectionStatus > *stati) const =0
Projects points in homogenous coordinates to 2d image points (projection) in a batch. Uses projection including distortion models.
virtual int noIntrinsicsParameters() const =0
Obtain the number of intrinsics parameters.
uint32_t imageHeight() const
The height of the image in pixels.
Definition: CameraBase.hpp:143
uint64_t id_
an Id
Definition: CameraBase.hpp:352
virtual ProjectionStatus project(const Eigen::Vector3d &point, Eigen::Vector2d *imagePoint) const =0
Projects a Euclidean point to a 2d image point (projection). Uses projection including distortion mod...
virtual void getIntrinsics(Eigen::VectorXd &intrinsics) const =0
obtain all intrinsics
virtual bool setIntrinsics(const Eigen::VectorXd &intrinsics)=0
overwrite all intrinsics - use with caution !
bool setMask(const cv::Mat &mask)
Set the mask. It must be the same size as the image and comply with OpenCV: 0 == masked, nonzero == valid. Type must be CV_8U1C.
Definition: CameraBase.hpp:47
Header implementation file for the CameraBase class.
virtual ProjectionStatus projectHomogeneous(const Eigen::Vector4d &point, Eigen::Vector2d *imagePoint) const =0
Projects a point in homogenous coordinates to a 2d image point (projection). Uses projection includin...
Header file for the DistortionBase class.
void setId(uint64_t id)
Set an Id.
Definition: CameraBase.hpp:124
bool hasMask() const
Was a nonzero mask set?
Definition: CameraBase.hpp:72
virtual bool backProjectHomogeneousBatch(const Eigen::Matrix2Xd &imagePoints, Eigen::Matrix4Xd *directions, std::vector< bool > *success) const =0
Back-project 2d image points into homogeneous points (direction vectors).
virtual bool backProjectHomogeneous(const Eigen::Vector2d &imagePoint, Eigen::Vector4d *direction) const =0
Back-project a 2d image point into homogeneous point (direction vector).
uint32_t imageWidth() const
The width of the image in pixels.
Definition: CameraBase.hpp:138
const cv::Mat & mask() const
Get the mask.
Definition: CameraBase.hpp:78
virtual Eigen::Vector3d createRandomVisiblePoint(double minDist=0.0, double maxDist=10.0) const
Creates a random visible point in Euclidean coordinates.
Definition: CameraBase.cpp:62
CameraBase()
default Constructor – does nothing serious
Definition: CameraBase.hpp:77
virtual bool backProject(const Eigen::Vector2d &imagePoint, Eigen::Vector3d *direction) const =0
Back-project a 2d image point into Euclidean space (direction vector).
virtual ~CameraBase()
Destructor – does nothing.
Definition: CameraBase.hpp:93
cv::Mat mask_
The mask – empty by default.
Definition: CameraBase.hpp:347
CameraBase(int imageWidth, int imageHeight, uint64_t id=0)
Constructor for width, height and Id.
Definition: CameraBase.hpp:85
bool isMasked(const Eigen::Vector2d &imagePoint) const
Check if the keypoint is masked.
Definition: CameraBase.hpp:83
Base class for all camera models.
Definition: CameraBase.hpp:60
virtual bool backProjectBatch(const Eigen::Matrix2Xd &imagePoints, Eigen::Matrix3Xd *directions, std::vector< bool > *success) const =0
Back-project 2d image points into Euclidean space (direction vectors).
virtual Eigen::Vector2d createRandomImagePoint() const
Creates a random (uniform distribution) image point.
Definition: CameraBase.cpp:47
virtual void projectBatch(const Eigen::Matrix3Xd &points, Eigen::Matrix2Xd *imagePoints, std::vector< ProjectionStatus > *stati) const =0
Projects Euclidean points to 2d image points (projection) in a batch. Uses projection including disto...
virtual 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 =0
Projects a point in homogenous coordinates to a 2d image point (projection). Uses projection includin...