OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
NCameraSystem.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  * Modified: Andreas Forster (an.forster@gmail.com)
32  *********************************************************************************/
33 
42 namespace okvis {
45 namespace cameras {
46 
47 // Default constructor
49 {
50 }
51 // Construct with vector of extrinsics and geometries
53  const std::vector<std::shared_ptr<const okvis::kinematics::Transformation>> & T_SC,
54  const std::vector<std::shared_ptr<const cameras::CameraBase>> & cameraGeometries,
55  const std::vector<DistortionType>& distortionTypes,
56  bool computeOverlaps)
57  : T_SC_(T_SC),
58  cameraGeometries_(cameraGeometries),
59  distortionTypes_(distortionTypes)
60 {
61 
63  Exception, T_SC.size() == cameraGeometries.size(),
64  "Number of extrinsics must match number of camera models!");
66  Exception, T_SC.size() == distortionTypes.size(),
67  "Number of distortiontypes must match number of extrinsics!");
68 
69  if (computeOverlaps) {
70  this->computeOverlaps();
71  }
72 }
74 {
75 }
76 
77 // Reset with vector of extrinsics and geometries
79  const std::vector<std::shared_ptr<const okvis::kinematics::Transformation>> & T_SC,
80  const std::vector<std::shared_ptr<const cameras::CameraBase>> & cameraGeometries,
81  const std::vector<DistortionType>& distortionTypes,
82  bool computeOverlaps)
83 {
85  Exception, T_SC.size() == cameraGeometries.size(),
86  "Number of extrinsics must match number of camera models!");
88  Exception, T_SC.size() == distortionTypes.size(),
89  "Number of distortiontypes must match number of extrinsics!");
90 
91  T_SC_ = T_SC;
92  cameraGeometries_ = cameraGeometries;
93  distortionTypes_ = distortionTypes;
94 
95  // recompute overlaps if requested
96  if (computeOverlaps) {
97  this->computeOverlaps();
98  }
99 }
100 
101 // Reset with vector of extrinsics and geometries
103  std::shared_ptr<const okvis::kinematics::Transformation> T_SC,
104  std::shared_ptr<const cameras::CameraBase> cameraGeometry,
105  DistortionType distortionType,
106  bool computeOverlaps)
107 {
108  T_SC_.push_back(T_SC);
109  cameraGeometries_.push_back(cameraGeometry);
110  distortionTypes_.push_back(distortionType);
111 
112  // recompute overlaps if requested
113  if (computeOverlaps) {
114  this->computeOverlaps();
115  }
116 }
117 
118 // get the pose of the IMU frame S with respect to the camera cameraIndex
119 std::shared_ptr<const okvis::kinematics::Transformation> NCameraSystem::T_SC(
120  size_t cameraIndex) const
121 {
122  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIndex < T_SC_.size(),
123  "Camera index " << cameraIndex << "out of range.");
124  return T_SC_[cameraIndex];
125 }
126 
127 //get the camera geometry of camera cameraIndex
128 std::shared_ptr<const cameras::CameraBase> NCameraSystem::cameraGeometry(
129  size_t cameraIndex) const
130 {
131  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIndex < cameraGeometries_.size(),
132  "Camera index " << cameraIndex << "out of range.");
133  return cameraGeometries_[cameraIndex];
134 }
135 
136 // get the distortion type of cmaera cameraIndex
138 {
139  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIndex < cameraGeometries_.size(),
140  "Camera index " << cameraIndex << "out of range.");
141  return distortionTypes_[cameraIndex];
142 }
143 
144 // Get the overlap mask
145 const cv::Mat NCameraSystem::overlap(size_t cameraIndexSeenBy,
146  size_t cameraIndex) const
147 {
149  Exception, cameraIndexSeenBy < T_SC_.size(),
150  "Camera index " << cameraIndexSeenBy << "out of range.");
151  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIndex < T_SC_.size(),
152  "Camera index " << cameraIndex << "out of range.");
153 
155  "Overlap computation not performed or incorrectly computed!");
156 
157  return overlapMats_[cameraIndexSeenBy][cameraIndex];
158 }
159 
160 // Can the first camera see parts of the FOV of the second camera?
161 bool NCameraSystem::hasOverlap(size_t cameraIndexSeenBy,
162  size_t cameraIndex) const
163 {
165  Exception, cameraIndexSeenBy < T_SC_.size(),
166  "Camera index " << cameraIndexSeenBy << "out of range.");
167  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIndex < T_SC_.size(),
168  "Camera index " << cameraIndex << "out of range.");
169  OKVIS_ASSERT_TRUE_DBG(Exception, cameraIndex < T_SC_.size(),
170  "Camera index " << cameraIndex << "out of range.");
171 
173  "Overlap computation not performed or incorrectly computed!");
174 
175  return overlaps_[cameraIndexSeenBy][cameraIndex];
176 }
177 
180  Exception, T_SC_.size() == cameraGeometries_.size(),
181  "Number of extrinsics must match number of camera models!");
182 
183  if(overlaps_.size() != cameraGeometries_.size()) {
184  return false;
185  }
186  if(overlapMats_.size() != cameraGeometries_.size()) {
187  return false;
188  }
189 
190  // also check for each element
191  for(size_t i= 0; i<overlaps_.size(); ++i){
192  if(overlaps_[i].size() != cameraGeometries_.size()) {
193  return false;
194  }
195  if(overlapMats_[i].size() != cameraGeometries_.size()) {
196  return false;
197  }
198  }
199  return true;
200 }
201 
203  return cameraGeometries_.size();
204 }
205 
206 } // namespace cameras
207 } // namespace okvis
void reset(const std::vector< std::shared_ptr< const okvis::kinematics::Transformation >> &T_SC, const std::vector< std::shared_ptr< const cameras::CameraBase >> &cameraGeometries, const std::vector< DistortionType > &distortionTypes, bool computeOverlaps)
Reset with vector of extrinsics and geometries.
Definition: NCameraSystem.hpp:78
void addCamera(std::shared_ptr< const okvis::kinematics::Transformation > T_SC, std::shared_ptr< const cameras::CameraBase > cameraGeometry, DistortionType distortionType, bool computeOverlaps=true)
Append with a single camera.
Definition: NCameraSystem.hpp:102
NCameraSystem()
Default constructor.
Definition: NCameraSystem.hpp:48
void computeOverlaps()
compute all the overlaps of fields of view. Attention: can be expensive.
Definition: NCameraSystem.cpp:50
std::vector< std::vector< bool > > overlaps_
Overlaps between cameras: binary.
Definition: NCameraSystem.hpp:154
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
std::vector< DistortionType > distortionTypes_
Definition: NCameraSystem.hpp:152
virtual ~NCameraSystem()
Destructor that doesn't do anything really.
Definition: NCameraSystem.hpp:73
std::vector< std::shared_ptr< const cameras::CameraBase > > cameraGeometries_
Camera geometries.
Definition: NCameraSystem.hpp:151
std::vector< std::vector< cv::Mat > > overlapMats_
Overlaps between cameras: mats.
Definition: NCameraSystem.hpp:153
DistortionType
The enumeration of the currently supported distortion types.
Definition: NCameraSystem.hpp:68
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
DistortionType distortionType(size_t cameraIndex) const
get the distortion type of the camera
Definition: NCameraSystem.hpp:137
std::vector< std::shared_ptr< const okvis::kinematics::Transformation > > T_SC_
Mounting transformations from IMU.
Definition: NCameraSystem.hpp:150
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 overlapComputationValid() const
Use this to check overlapMats_ and overlaps_ have correct sizes.
Definition: NCameraSystem.hpp:178
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