OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
MultiFrame.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: Mar 30, 2015
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  * Modified: Andreas Forster (an.forster@gmail.com)
32  *********************************************************************************/
33 
41 #ifndef INCLUDE_OKVIS_MULTIFRAME_HPP_
42 #define INCLUDE_OKVIS_MULTIFRAME_HPP_
43 
44 #include <memory>
45 #include <okvis/assert_macros.hpp>
46 #include <okvis/Frame.hpp>
48 
50 namespace okvis {
51 
55 {
56  public:
57 
58  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
59  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
60 
61 
62  inline MultiFrame();
63 
68  inline MultiFrame(const cameras::NCameraSystem & cameraSystem,
69  const okvis::Time & timestamp, uint64_t id = 0);
70 
72  inline virtual ~MultiFrame();
73 
76  inline void resetCameraSystemAndFrames(
77  const cameras::NCameraSystem & cameraSystem);
78 
81  inline void setTimestamp(const okvis::Time & timestamp);
82 
85  inline void setId(uint64_t id);
86 
89  inline const okvis::Time & timestamp() const;
90 
93  inline uint64_t id() const;
94 
97  inline size_t numFrames() const;
98 
102  inline std::shared_ptr<const okvis::kinematics::Transformation> T_SC(
103  size_t cameraIdx) const;
104 
108 
112  inline void setImage(size_t cameraIdx, const cv::Mat & image);
113 
117  inline void setGeometry(
118  size_t cameraIdx,
119  std::shared_ptr<const cameras::CameraBase> cameraGeometry);
120 
124  inline void setDetector(size_t cameraIdx,
125  std::shared_ptr<cv::FeatureDetector> detector);
126 
130  inline void setExtractor(
131  size_t cameraIdx,
132  std::shared_ptr<cv::DescriptorExtractor> extractor);
133 
137  inline const cv::Mat & image(size_t cameraIdx) const;
138 
142  inline std::shared_ptr<const cameras::CameraBase> geometry(
143  size_t cameraIdx) const;
144 
149  template<class GEOMETRY_T>
150  inline std::shared_ptr<const GEOMETRY_T> geometryAs(size_t cameraIdx) const;
151 
155  inline int detect(size_t cameraIdx);
156 
162  inline int describe(size_t cameraIdx,
163  const Eigen::Vector3d & extractionDirection =
164  Eigen::Vector3d(0, 0, 1));
171  template<class GEOMETRY_T>
172  inline int describeAs(size_t cameraIdx,
173  const Eigen::Vector3d & extractionDirection =
174  Eigen::Vector3d(0, 0, 1));
175 
181  inline bool getCvKeypoint(size_t cameraIdx, size_t keypointIdx,
182  cv::KeyPoint & keypoint) const;
183 
189  inline bool getKeypoint(size_t cameraIdx, size_t keypointIdx,
190  Eigen::Vector2d & keypoint) const;
191 
197  inline bool getKeypointSize(size_t cameraIdx, size_t keypointIdx,
198  double & keypointSize) const;
199 
204  inline const unsigned char * keypointDescriptor(size_t cameraIdx,
205  size_t keypointIdx);
206 
212  inline bool setLandmarkId(size_t cameraIdx, size_t keypointIdx,
213  uint64_t landmarkId);
214 
219  inline uint64_t landmarkId(size_t cameraIdx, size_t keypointIdx) const;
220 
225  inline bool resetKeypoints(size_t cameraIdx,
226  const std::vector<cv::KeyPoint> & keypoints);
227 
232  inline bool resetDescriptors(size_t cameraIdx, const cv::Mat & descriptors);
233 
237  inline size_t numKeypoints(size_t cameraIdx) const;
238 
240 
243  inline size_t numKeypoints() const;
244 
250  inline const cv::Mat overlap(size_t cameraIndexSeenBy,
251  size_t cameraIndex) const
252  {
253  return cameraSystem_.overlap(cameraIndexSeenBy, cameraIndex);
254  }
255 
260  inline bool hasOverlap(size_t cameraIndexSeenBy, size_t cameraIndex) const
261  {
262  return cameraSystem_.hasOverlap(cameraIndexSeenBy, cameraIndex);
263  }
264 
265  protected:
267  uint64_t id_;
268  std::vector<Frame, Eigen::aligned_allocator<Frame>> frames_;
270 };
271 
272 typedef std::shared_ptr<MultiFrame> MultiFramePtr;
273 
274 } // namespace okvis
275 
277 
278 #endif /* INCLUDE_OKVIS_MULTIFRAME_HPP_ */
Header file for the NCameraSystem class.
bool setLandmarkId(size_t cameraIdx, size_t keypointIdx, uint64_t landmarkId)
Set the landmark ID.
Definition: MultiFrame.hpp:227
bool hasOverlap(size_t cameraIndexSeenBy, size_t cameraIndex) const
Can the first camera see parts of the FOV of the second camera?
Definition: MultiFrame.hpp:260
uint64_t landmarkId(size_t cameraIdx, size_t keypointIdx) const
Access the landmark ID.
Definition: MultiFrame.hpp:235
void resetCameraSystemAndFrames(const cameras::NCameraSystem &cameraSystem)
(Re)set the NCameraSystem – which clears the frames as well.
Definition: MultiFrame.hpp:60
bool resetKeypoints(size_t cameraIdx, const std::vector< cv::KeyPoint > &keypoints)
provide keypoints externally
Definition: MultiFrame.hpp:249
void setId(uint64_t id)
(Re)set the id
Definition: MultiFrame.hpp:80
std::shared_ptr< const okvis::kinematics::Transformation > T_SC(size_t cameraIdx) const
Get the extrinsics of a camera.
Definition: MultiFrame.hpp:103
std::vector< Frame, Eigen::aligned_allocator< Frame > > frames_
the individual frames
Definition: MultiFrame.hpp:268
const unsigned char * keypointDescriptor(size_t cameraIdx, size_t keypointIdx)
Access the descriptor – CAUTION: high-speed version.
Definition: MultiFrame.hpp:219
Header implementation file for the MultiFrame class.
bool getKeypointSize(size_t cameraIdx, size_t keypointIdx, double &keypointSize) const
Get the size of a specific keypoint.
Definition: MultiFrame.hpp:210
void setImage(size_t cameraIdx, const cv::Mat &image)
Set the frame image;.
Definition: MultiFrame.hpp:113
const cv::Mat overlap(size_t cameraIndexSeenBy, size_t cameraIndex) const
Get the overlap mask. Sorry for the weird syntax, but remember that cv::Mat is essentially a shared p...
Definition: MultiFrame.hpp:250
void setGeometry(size_t cameraIdx, std::shared_ptr< const cameras::CameraBase > cameraGeometry)
Set the geometry.
Definition: MultiFrame.hpp:120
Header file for the Frame class.
std::shared_ptr< MultiFrame > MultiFramePtr
For convenience.
Definition: MultiFrame.hpp:272
A class that assembles multiple cameras into a system of (potentially different) cameras.
Definition: NCameraSystem.hpp:61
std::shared_ptr< const GEOMETRY_T > geometryAs(size_t cameraIdx) const
Get the specific geometry (will be fast to use)
Definition: MultiFrame.hpp:160
const cv::Mat & image(size_t cameraIdx) const
Obtain the image.
Definition: MultiFrame.hpp:144
uint64_t id_
the frame id
Definition: MultiFrame.hpp:267
bool resetDescriptors(size_t cameraIdx, const cv::Mat &descriptors)
provide descriptors externally
Definition: MultiFrame.hpp:255
#define OKVIS_DEFINE_EXCEPTION(exceptionName, exceptionParent)
Macro for defining an exception with a given parent.
Definition: assert_macros.hpp:52
int describe(size_t cameraIdx, const Eigen::Vector3d &extractionDirection=Eigen::Vector3d(0, 0, 1))
Describe keypoints. This uses virtual function calls. That's a negligibly small overhead for many det...
Definition: MultiFrame.hpp:179
This file contains some useful assert macros.
void setDetector(size_t cameraIdx, std::shared_ptr< cv::FeatureDetector > detector)
Set the detector.
Definition: MultiFrame.hpp:128
bool getCvKeypoint(size_t cameraIdx, size_t keypointIdx, cv::KeyPoint &keypoint) const
Access a specific keypoint in OpenCV format.
Definition: MultiFrame.hpp:194
int describeAs(size_t cameraIdx, const Eigen::Vector3d &extractionDirection=Eigen::Vector3d(0, 0, 1))
Describe keypoints. This uses virtual function calls. That's a negligibly small overhead for many det...
Definition: MultiFrame.hpp:186
size_t numFrames() const
The number of frames/cameras.
Definition: MultiFrame.hpp:98
size_t numKeypoints() const
Get the total number of keypoints in all frames.
Definition: MultiFrame.hpp:263
virtual ~MultiFrame()
Destructor...
Definition: MultiFrame.hpp:54
int detect(size_t cameraIdx)
Detect keypoints. This uses virtual function calls. That's a negligibly small overhead for many detec...
Definition: MultiFrame.hpp:169
EIGEN_MAKE_ALIGNED_OPERATOR_NEW MultiFrame()
Default constructor.
Definition: MultiFrame.hpp:43
void setExtractor(size_t cameraIdx, std::shared_ptr< cv::DescriptorExtractor > extractor)
Set the extractor.
Definition: MultiFrame.hpp:136
void setTimestamp(const okvis::Time &timestamp)
(Re)set the timestamp
Definition: MultiFrame.hpp:74
const cv::Mat overlap(size_t cameraIndexSeenBy, size_t cameraIndex) const
Get the overlap mask. Sorry for the weird syntax, but remember that cv::Mat is essentially a shared p...
Definition: NCameraSystem.hpp:145
A multi camera frame that uses okvis::Frame underneath.
Definition: MultiFrame.hpp:54
okvis::Time timestamp_
the frame timestamp
Definition: MultiFrame.hpp:266
uint64_t id() const
Obtain the frame id.
Definition: MultiFrame.hpp:92
std::shared_ptr< const cameras::CameraBase > geometry(size_t cameraIdx) const
get the base class geometry (will be slow to use)
Definition: MultiFrame.hpp:151
Time representation. May either represent wall clock time or ROS clock time.
Definition: Time.hpp:187
bool hasOverlap(size_t cameraIndexSeenBy, size_t cameraIndex) const
Can the first camera see parts of the FOV of the second camera?
Definition: NCameraSystem.hpp:161
bool getKeypoint(size_t cameraIdx, size_t keypointIdx, Eigen::Vector2d &keypoint) const
Get a specific keypoint.
Definition: MultiFrame.hpp:202
const okvis::Time & timestamp() const
Obtain the frame timestamp.
Definition: MultiFrame.hpp:86
cameras::NCameraSystem cameraSystem_
the camera system
Definition: MultiFrame.hpp:269