OKVIS
 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: Apr 1, 2015
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 namespace okvis {
41 
42 // Default constructor
44 
45 // Construct from NCameraSystem
47  const okvis::Time & timestamp, uint64_t id)
48  : timestamp_(timestamp),
49  id_(id)
50 {
51  resetCameraSystemAndFrames(cameraSystem);
52 }
53 
55 {
56 
57 }
58 
59 // (Re)set the NCameraSystem -- which clears the frames as well.
61  const cameras::NCameraSystem & cameraSystem)
62 {
63  cameraSystem_ = cameraSystem;
64  frames_.clear(); // erase -- for safety
65  frames_.resize(cameraSystem.numCameras());
66 
67  // copy cameras
68  for(size_t c = 0; c<numFrames(); ++c){
69  frames_[c].setGeometry(cameraSystem.cameraGeometry(c));
70  }
71 }
72 
73 // (Re)set the timestamp
74 void MultiFrame::setTimestamp(const okvis::Time & timestamp)
75 {
77 }
78 
79 // (Re)set the id
80 void MultiFrame::setId(uint64_t id)
81 {
82  id_ = id;
83 }
84 
85 // Obtain the frame timestamp
87 {
88  return timestamp_;
89 }
90 
91 // Obtain the frame id
92 uint64_t MultiFrame::id() const
93 {
94  return id_;
95 }
96 
97 // The number of frames/cameras
98 size_t MultiFrame::numFrames() const
99 {
100  return frames_.size();
101 }
102 
103 std::shared_ptr<const okvis::kinematics::Transformation> MultiFrame::T_SC(size_t cameraIdx) const {
104  return cameraSystem_.T_SC(cameraIdx);
105 }
106 
107 
109 // The following mirror the Frame functionality.
110 //
111 
112 // Set the frame image;
113 void MultiFrame::setImage(size_t cameraIdx, const cv::Mat & image)
114 {
115  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
116  frames_[cameraIdx].setImage(image);
117 }
118 
119 // Set the geometry
121  size_t cameraIdx, std::shared_ptr<const cameras::CameraBase> cameraGeometry)
122 {
123  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
124  frames_[cameraIdx].setGeometry(cameraGeometry);
125 }
126 
127 // Set the detector
128 void MultiFrame::setDetector(size_t cameraIdx,
129  std::shared_ptr<cv::FeatureDetector> detector)
130 {
131  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
132  frames_[cameraIdx].setDetector(detector);
133 }
134 
135 // Set the extractor
137  size_t cameraIdx, std::shared_ptr<cv::DescriptorExtractor> extractor)
138 {
139  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
140  frames_[cameraIdx].setExtractor(extractor);
141 }
142 
143 // Obtain the image
144 const cv::Mat & MultiFrame::image(size_t cameraIdx) const
145 {
146  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
147  return frames_[cameraIdx].image();
148 }
149 
150 // get the base class geometry (will be slow to use)
151 std::shared_ptr<const cameras::CameraBase> MultiFrame::geometry(
152  size_t cameraIdx) const
153 {
154  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
155  return frames_[cameraIdx].geometry();
156 }
157 
158 // Get the specific geometry (will be fast to use)
159 template<class GEOMETRY_T>
160 std::shared_ptr<const GEOMETRY_T> MultiFrame::geometryAs(size_t cameraIdx) const
161 {
162  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
163  return frames_[cameraIdx].geometryAs<GEOMETRY_T>();
164 }
165 
166 // Detect keypoints. This uses virtual function calls.
169 int MultiFrame::detect(size_t cameraIdx)
170 {
171  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
172  return frames_[cameraIdx].detect();
173 }
174 
175 // Describe keypoints. This uses virtual function calls.
179 int MultiFrame::describe(size_t cameraIdx,
180  const Eigen::Vector3d & extractionDirection)
181 {
182  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
183  return frames_[cameraIdx].describe(extractionDirection);
184 }
185 template<class GEOMETRY_T>
186 int MultiFrame::describeAs(size_t cameraIdx,
187  const Eigen::Vector3d & extractionDirection)
188 {
189  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
190  return frames_[cameraIdx].template describeAs <GEOMETRY_T> (extractionDirection);
191 }
192 
193 // Access a specific keypoint in OpenCV format
194 bool MultiFrame::getCvKeypoint(size_t cameraIdx, size_t keypointIdx,
195  cv::KeyPoint & keypoint) const
196 {
197  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
198  return frames_[cameraIdx].getCvKeypoint(keypointIdx, keypoint);
199 }
200 
201 // Get a specific keypoint
202 bool MultiFrame::getKeypoint(size_t cameraIdx, size_t keypointIdx,
203  Eigen::Vector2d & keypoint) const
204 {
205  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
206  return frames_[cameraIdx].getKeypoint(keypointIdx, keypoint);
207 }
208 
209 // Get the size of a specific keypoint
210 bool MultiFrame::getKeypointSize(size_t cameraIdx, size_t keypointIdx,
211  double & keypointSize) const
212 {
213  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
214  return frames_[cameraIdx].getKeypointSize(keypointIdx, keypointSize);
215 }
216 
217 // Access the descriptor -- CAUTION: high-speed version.
219 const unsigned char * MultiFrame::keypointDescriptor(size_t cameraIdx,
220  size_t keypointIdx)
221 {
222  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
223  return frames_[cameraIdx].keypointDescriptor(keypointIdx);
224 }
225 
226 // Set the landmark ID
227 bool MultiFrame::setLandmarkId(size_t cameraIdx, size_t keypointIdx,
228  uint64_t landmarkId)
229 {
230  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
231  return frames_[cameraIdx].setLandmarkId(keypointIdx, landmarkId);
232 }
233 
234 // Access the landmark ID
235 uint64_t MultiFrame::landmarkId(size_t cameraIdx, size_t keypointIdx) const
236 {
237  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
238  return frames_[cameraIdx].landmarkId(keypointIdx);
239 }
240 
241 // number of keypoints
242 size_t MultiFrame::numKeypoints(size_t cameraIdx) const
243 {
244  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
245  return frames_[cameraIdx].numKeypoints();
246 }
247 
248 // provide keypoints externally
249 bool MultiFrame::resetKeypoints(size_t cameraIdx, const std::vector<cv::KeyPoint> & keypoints){
250  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
251  return frames_[cameraIdx].resetKeypoints(keypoints);
252 }
253 
254 // provide descriptors externally
255 bool MultiFrame::resetDescriptors(size_t cameraIdx, const cv::Mat & descriptors) {
256  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIdx < frames_.size(), "Out of range");
257  return frames_[cameraIdx].resetDescriptors(descriptors);
258 }
259 
260 //
261 
262 // get the total number of keypoints in all frames.
264 {
265  size_t numKeypoints = 0;
266  for (size_t i = 0; i < frames_.size(); ++i) {
267  numKeypoints += frames_[i].numKeypoints();
268  }
269  return numKeypoints;
270 }
271 
272 
273 }// namespace okvis
bool setLandmarkId(size_t cameraIdx, size_t keypointIdx, uint64_t landmarkId)
Set the landmark ID.
Definition: MultiFrame.hpp:227
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
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
void setGeometry(size_t cameraIdx, std::shared_ptr< const cameras::CameraBase > cameraGeometry)
Set the geometry.
Definition: MultiFrame.hpp:120
std::shared_ptr< const okvis::kinematics::Transformation > T_SC(size_t cameraIndex) const
get the pose of the IMU frame S with respect to the camera cameraIndex
Definition: NCameraSystem.hpp:119
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
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
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
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 getKeypoint(size_t cameraIdx, size_t keypointIdx, Eigen::Vector2d &keypoint) const
Get a specific keypoint.
Definition: MultiFrame.hpp:202
size_t numCameras() const
Obtatin the number of cameras currently added.
Definition: NCameraSystem.hpp:202
#define OKVIS_ASSERT_TRUE_DBG(exceptionType, condition, message)
Definition: assert_macros.hpp:211
std::shared_ptr< const cameras::CameraBase > cameraGeometry(size_t cameraIndex) const
get the camera geometry of camera cameraIndex
Definition: NCameraSystem.hpp:128
const okvis::Time & timestamp() const
Obtain the frame timestamp.
Definition: MultiFrame.hpp:86
cameras::NCameraSystem cameraSystem_
the camera system
Definition: MultiFrame.hpp:269