OKVIS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Frame.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 31, 2015
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  * Modified: Andreas Forster (an.forster@gmail.com)
32  *********************************************************************************/
33 
41 namespace okvis {
43 
44 // a constructor that uses the specified geometry,
46 Frame::Frame(const cv::Mat & image,
47  std::shared_ptr<cameras::CameraBase> & cameraGeometry,
48  std::shared_ptr<cv::FeatureDetector> & detector,
49  std::shared_ptr<cv::DescriptorExtractor> & extractor)
50  : image_(image),
51  cameraGeometry_(cameraGeometry),
52  detector_(detector),
53  extractor_(extractor)
54 {
55 }
56 
57 // set the frame image;
58 void Frame::setImage(const cv::Mat & image)
59 {
60  image_ = image;
61 }
62 
63 // set the geometry
64 void Frame::setGeometry(std::shared_ptr<const cameras::CameraBase> cameraGeometry)
65 {
66  cameraGeometry_ = cameraGeometry;
67 }
68 
69 // set the detector
70 void Frame::setDetector(std::shared_ptr<cv::FeatureDetector> detector)
71 {
72  detector_ = detector;
73 }
74 
75 // set the extractor
76 void Frame::setExtractor(std::shared_ptr<cv::DescriptorExtractor> extractor)
77 {
78  extractor_ = extractor;
79 }
80 
81 // obtain the image
82 const cv::Mat & Frame::image() const
83 {
84  return image_;
85 }
86 
87 // get the base class geometry (will be slow to use)
88 std::shared_ptr<const cameras::CameraBase> Frame::geometry() const
89 {
90  return cameraGeometry_;
91 }
92 
93 // get the specific geometry (will be fast to use)
94 template<class GEOMETRY_T>
95 std::shared_ptr<const GEOMETRY_T> Frame::geometryAs() const
96 {
97 #ifndef NDEBUG
99  Exception, std::dynamic_pointer_cast<const GEOMETRY_T>(cameraGeometry_),
100  "incorrect pointer cast requested. " << cameraGeometry_->distortionType());
101  return std::static_pointer_cast<const GEOMETRY_T>(cameraGeometry_);
102 #else
103  return std::static_pointer_cast<const GEOMETRY_T>(cameraGeometry_);
104 #endif
105 }
106 
107 // detect keypoints. This uses virtual function calls.
111 {
112  // make sure things are set to zero for safety
113  keypoints_.clear();
114  descriptors_.resize(0);
115  landmarkIds_.clear(); // resizing and filling in zeros in Frame::describe() as some keypoints are removed there.
116 
117  // run the detector
118  OKVIS_ASSERT_TRUE_DBG(Exception, detector_ != NULL,
119  "Detector not initialised!");
120  detector_->detect(image_, keypoints_);
121  return keypoints_.size();
122 }
123 
124 // describe keypoints. This uses virtual function calls.
128 int Frame::describe(const Eigen::Vector3d & extractionDirection)
129 {
130  // check initialisation
131  OKVIS_ASSERT_TRUE_DBG(Exception, extractor_ != NULL,
132  "Detector not initialised!");
133 
134  // orient the keypoints according to the extraction direction:
135  Eigen::Vector3d ep;
136  Eigen::Vector2d reprojection;
137  Eigen::Matrix<double, 2, 3> Jacobian;
138  Eigen::Vector2d eg_projected;
139  for (size_t k = 0; k < keypoints_.size(); ++k) {
140  cv::KeyPoint& ckp = keypoints_[k];
141  // project ray
142  cameraGeometry_->backProject(Eigen::Vector2d(ckp.pt.x, ckp.pt.y), &ep);
143  // obtain image Jacobian
144  cameraGeometry_->project(ep, &reprojection, &Jacobian);
145  // multiply with gravity direction
146  eg_projected = Jacobian * extractionDirection;
147  double angle = atan2(eg_projected[1], eg_projected[0]);
148  // set
149  ckp.angle = angle / M_PI * 180.0;
150  }
151 
152  // extraction
154  landmarkIds_ = std::vector<uint64_t>(keypoints_.size(),0);
155  return keypoints_.size();
156 }
157 // describe keypoints. This uses virtual function calls.
161 template<class GEOMETRY_T>
162 int Frame::describeAs(const Eigen::Vector3d & extractionDirection)
163 {
164  // check initialisation
165  OKVIS_ASSERT_TRUE_DBG(Exception, extractor_ != NULL,
166  "Detector not initialised!");
167 
168  // orient the keypoints according to the extraction direction:
169  Eigen::Vector3d ep;
170  Eigen::Vector2d reprojection;
171  Eigen::Matrix<double, 2, 3> Jacobian;
172  Eigen::Vector2d eg_projected;
173  for (size_t k = 0; k < keypoints_.size(); ++k) {
174  cv::KeyPoint& ckp = keypoints_[k];
175  // project ray
176  geometryAs<GEOMETRY_T>()->backProject(Eigen::Vector2d(ckp.pt.x, ckp.pt.y),
177  &ep);
178  // obtain image Jacobian
179  geometryAs<GEOMETRY_T>()->project(ep, &reprojection, &Jacobian);
180  // multiply with gravity direction
181  eg_projected = Jacobian * extractionDirection;
182  double angle = atan2(eg_projected[1], eg_projected[0]);
183  // set
184  ckp.angle = angle / M_PI * 180.0;
185  }
186 
187  // extraction
189  return keypoints_.size();
190 }
191 
192 // access a specific keypoint in OpenCV format
193 bool Frame::getCvKeypoint(size_t keypointIdx, cv::KeyPoint & keypoint) const
194 {
195 #ifndef NDEBUG
197  Exception,
198  keypointIdx < keypoints_.size(),
199  "keypointIdx " << keypointIdx << "out of range: keypoints has size "
200  << keypoints_.size());
201  keypoint = keypoints_[keypointIdx];
202  return keypointIdx < keypoints_.size();
203 #else
204  keypoint = keypoints_[keypointIdx];
205  return true;
206 #endif
207 }
208 
209 // get a specific keypoint
210 bool Frame::getKeypoint(size_t keypointIdx, Eigen::Vector2d & keypoint) const
211 {
212 #ifndef NDEBUG
214  Exception,
215  keypointIdx < keypoints_.size(),
216  "keypointIdx " << keypointIdx << "out of range: keypoints has size "
217  << keypoints_.size());
218  keypoint = Eigen::Vector2d(keypoints_[keypointIdx].pt.x,
219  keypoints_[keypointIdx].pt.y);
220  return keypointIdx < keypoints_.size();
221 #else
222  keypoint = Eigen::Vector2d(keypoints_[keypointIdx].pt.x, keypoints_[keypointIdx].pt.y);
223  return true;
224 #endif
225 }
226 
227 // get the size of a specific keypoint
228 bool Frame::getKeypointSize(size_t keypointIdx, double & keypointSize) const
229 {
230 #ifndef NDEBUG
232  Exception,
233  keypointIdx < keypoints_.size(),
234  "keypointIdx " << keypointIdx << "out of range: keypoints has size "
235  << keypoints_.size());
236  keypointSize = keypoints_[keypointIdx].size;
237  return keypointIdx < keypoints_.size();
238 #else
239  keypointSize = keypoints_[keypointIdx].size;
240  return true;
241 #endif
242 }
243 
244 // access the descriptor -- CAUTION: high-speed version.
246 const unsigned char * Frame::keypointDescriptor(size_t keypointIdx)
247 {
248 #ifndef NDEBUG
250  Exception,
251  keypointIdx < keypoints_.size(),
252  "keypointIdx " << keypointIdx << "out of range: keypoints has size "
253  << keypoints_.size());
254  return descriptors_.data + descriptors_.cols * keypointIdx;
255 #else
256  return descriptors_.data + descriptors_.cols * keypointIdx;
257 #endif
258 }
259 
260 // Set the landmark ID
261 bool Frame::setLandmarkId(size_t keypointIdx, uint64_t landmarkId)
262 {
263 #ifndef NDEBUG
265  Exception,
266  keypointIdx < landmarkIds_.size(),
267  "keypointIdx " << keypointIdx << "out of range: landmarkIds_ has size "
268  << landmarkIds_.size());
269  landmarkIds_[keypointIdx] = landmarkId;
270  return keypointIdx < keypoints_.size();
271 #else
272  landmarkIds_[keypointIdx] = landmarkId;
273  return true;
274 #endif
275 }
276 
277 // Access the landmark ID
278 uint64_t Frame::landmarkId(size_t keypointIdx) const
279 {
280 #ifndef NDEBUG
282  Exception,
283  keypointIdx < landmarkIds_.size(),
284  "keypointIdx " << keypointIdx << "out of range: landmarkIds has size "
285  << landmarkIds_.size());
286  return landmarkIds_[keypointIdx];
287 #else
288  return landmarkIds_[keypointIdx];
289 #endif
290 }
291 
292 // provide keypoints externally
293 inline bool Frame::resetKeypoints(const std::vector<cv::KeyPoint> & keypoints) {
294  keypoints_ = keypoints;
295  landmarkIds_ = std::vector<uint64_t>(keypoints_.size(),0);
296  return true;
297 }
298 
299 // provide descriptors externally
300 inline bool Frame::resetDescriptors(const cv::Mat & descriptors) {
301  descriptors_ = descriptors;
302  return true;
303 }
304 
305 size_t Frame::numKeypoints() const {
306  return keypoints_.size();
307 }
308 
309 } // namespace okvis
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Frame()
a default constructor
Definition: Frame.hpp:71
uint64_t landmarkId(size_t keypointIdx) const
Access the landmark ID.
Definition: Frame.hpp:278
std::shared_ptr< const cameras::CameraBase > cameraGeometry_
the camera geometry
Definition: Frame.hpp:192
int describe(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: Frame.hpp:128
cv::Mat descriptors_
we store the descriptors using OpenCV's matrices
Definition: Frame.hpp:196
cv::Mat image_
the image as OpenCV's matrix
Definition: Frame.hpp:191
bool resetDescriptors(const cv::Mat &descriptors)
provide descriptors externally
Definition: Frame.hpp:300
std::vector< uint64_t > landmarkIds_
landmark Id, if associated – 0 otherwise
Definition: Frame.hpp:197
int detect()
Detect keypoints. This uses virtual function calls. That's a negligibly small overhead for many detec...
Definition: Frame.hpp:110
bool getKeypoint(size_t keypointIdx, Eigen::Vector2d &keypoint) const
Get a specific keypoint.
Definition: Frame.hpp:210
const cv::Mat & image() const
Obtain the image.
Definition: Frame.hpp:82
bool getCvKeypoint(size_t keypointIdx, cv::KeyPoint &keypoint) const
Access a specific keypoint in OpenCV format.
Definition: Frame.hpp:193
std::shared_ptr< cv::FeatureDetector > detector_
the detector
Definition: Frame.hpp:193
bool getKeypointSize(size_t keypointIdx, double &keypointSize) const
Get the size of a specific keypoint.
Definition: Frame.hpp:228
std::vector< cv::KeyPoint > keypoints_
we store keypoints using OpenCV's struct
Definition: Frame.hpp:195
int describeAs(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: Frame.hpp:162
std::shared_ptr< cv::DescriptorExtractor > extractor_
the extractor
Definition: Frame.hpp:194
std::shared_ptr< const cameras::CameraBase > geometry() const
get the base class geometry (will be slow to use)
Definition: Frame.hpp:88
void setExtractor(std::shared_ptr< cv::DescriptorExtractor > extractor)
Set the extractor.
Definition: Frame.hpp:76
bool resetKeypoints(const std::vector< cv::KeyPoint > &keypoints)
Provide keypoints externally.
Definition: Frame.hpp:293
const unsigned char * keypointDescriptor(size_t keypointIdx)
Access the descriptor – CAUTION: high-speed version.
Definition: Frame.hpp:246
bool setLandmarkId(size_t keypointIdx, uint64_t landmarkId)
Set the landmark ID.
Definition: Frame.hpp:261
std::shared_ptr< const GEOMETRY_T > geometryAs() const
Get the specific geometry (will be fast to use)
Definition: Frame.hpp:95
size_t numKeypoints() const
Get the number of keypoints.
Definition: Frame.hpp:305
void setDetector(std::shared_ptr< cv::FeatureDetector > detector)
Set the detector.
Definition: Frame.hpp:70
#define OKVIS_ASSERT_TRUE_DBG(exceptionType, condition, message)
Definition: assert_macros.hpp:211
void setGeometry(std::shared_ptr< const cameras::CameraBase > cameraGeometry)
Set the geometry.
Definition: Frame.hpp:64
void setImage(const cv::Mat &image)
Set the frame image;.
Definition: Frame.hpp:58
#define OKVIS_ASSERT_TRUE(exceptionType, condition, message)
Definition: assert_macros.hpp:111