1 // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 //
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 //
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 //
14 // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
15 // its contributors may be used to endorse or promote products derived
16 // from this software without specific prior written permission.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
22 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 // POSSIBILITY OF SUCH DAMAGE.
29 //
30 // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
31
32 #include "base/camera.h"
33
34 #include <iomanip>
35
36 #include "base/camera_models.h"
37 #include "util/logging.h"
38 #include "util/misc.h"
39
40 namespace colmap {
41
Camera()42 Camera::Camera()
43 : camera_id_(kInvalidCameraId),
44 model_id_(kInvalidCameraModelId),
45 width_(0),
46 height_(0),
47 prior_focal_length_(false) {}
48
ModelName() const49 std::string Camera::ModelName() const { return CameraModelIdToName(model_id_); }
50
SetModelId(const int model_id)51 void Camera::SetModelId(const int model_id) {
52 CHECK(ExistsCameraModelWithId(model_id));
53 model_id_ = model_id;
54 params_.resize(CameraModelNumParams(model_id_), 0);
55 }
56
SetModelIdFromName(const std::string & model_name)57 void Camera::SetModelIdFromName(const std::string& model_name) {
58 CHECK(ExistsCameraModelWithName(model_name));
59 model_id_ = CameraModelNameToId(model_name);
60 params_.resize(CameraModelNumParams(model_id_), 0);
61 }
62
FocalLengthIdxs() const63 const std::vector<size_t>& Camera::FocalLengthIdxs() const {
64 return CameraModelFocalLengthIdxs(model_id_);
65 }
66
PrincipalPointIdxs() const67 const std::vector<size_t>& Camera::PrincipalPointIdxs() const {
68 return CameraModelPrincipalPointIdxs(model_id_);
69 }
70
ExtraParamsIdxs() const71 const std::vector<size_t>& Camera::ExtraParamsIdxs() const {
72 return CameraModelExtraParamsIdxs(model_id_);
73 }
74
CalibrationMatrix() const75 Eigen::Matrix3d Camera::CalibrationMatrix() const {
76 Eigen::Matrix3d K = Eigen::Matrix3d::Identity();
77
78 const std::vector<size_t>& idxs = FocalLengthIdxs();
79 if (idxs.size() == 1) {
80 K(0, 0) = params_[idxs[0]];
81 K(1, 1) = params_[idxs[0]];
82 } else if (idxs.size() == 2) {
83 K(0, 0) = params_[idxs[0]];
84 K(1, 1) = params_[idxs[1]];
85 } else {
86 LOG(FATAL)
87 << "Camera model must either have 1 or 2 focal length parameters.";
88 }
89
90 K(0, 2) = PrincipalPointX();
91 K(1, 2) = PrincipalPointY();
92
93 return K;
94 }
95
ParamsInfo() const96 std::string Camera::ParamsInfo() const {
97 return CameraModelParamsInfo(model_id_);
98 }
99
MeanFocalLength() const100 double Camera::MeanFocalLength() const {
101 const auto& focal_length_idxs = FocalLengthIdxs();
102 double focal_length = 0;
103 for (const auto idx : focal_length_idxs) {
104 focal_length += params_[idx];
105 }
106 return focal_length / focal_length_idxs.size();
107 }
108
FocalLength() const109 double Camera::FocalLength() const {
110 const std::vector<size_t>& idxs = FocalLengthIdxs();
111 CHECK_EQ(idxs.size(), 1);
112 return params_[idxs[0]];
113 }
114
FocalLengthX() const115 double Camera::FocalLengthX() const {
116 const std::vector<size_t>& idxs = FocalLengthIdxs();
117 CHECK_EQ(idxs.size(), 2);
118 return params_[idxs[0]];
119 }
120
FocalLengthY() const121 double Camera::FocalLengthY() const {
122 const std::vector<size_t>& idxs = FocalLengthIdxs();
123 CHECK_EQ(idxs.size(), 2);
124 return params_[idxs[1]];
125 }
126
SetFocalLength(const double focal_length)127 void Camera::SetFocalLength(const double focal_length) {
128 const std::vector<size_t>& idxs = FocalLengthIdxs();
129 for (const auto idx : idxs) {
130 params_[idx] = focal_length;
131 }
132 }
133
SetFocalLengthX(const double focal_length_x)134 void Camera::SetFocalLengthX(const double focal_length_x) {
135 const std::vector<size_t>& idxs = FocalLengthIdxs();
136 CHECK_EQ(idxs.size(), 2);
137 params_[idxs[0]] = focal_length_x;
138 }
139
SetFocalLengthY(const double focal_length_y)140 void Camera::SetFocalLengthY(const double focal_length_y) {
141 const std::vector<size_t>& idxs = FocalLengthIdxs();
142 CHECK_EQ(idxs.size(), 2);
143 params_[idxs[1]] = focal_length_y;
144 }
145
PrincipalPointX() const146 double Camera::PrincipalPointX() const {
147 const std::vector<size_t>& idxs = PrincipalPointIdxs();
148 CHECK_EQ(idxs.size(), 2);
149 return params_[idxs[0]];
150 }
151
PrincipalPointY() const152 double Camera::PrincipalPointY() const {
153 const std::vector<size_t>& idxs = PrincipalPointIdxs();
154 CHECK_EQ(idxs.size(), 2);
155 return params_[idxs[1]];
156 }
157
SetPrincipalPointX(const double ppx)158 void Camera::SetPrincipalPointX(const double ppx) {
159 const std::vector<size_t>& idxs = PrincipalPointIdxs();
160 CHECK_EQ(idxs.size(), 2);
161 params_[idxs[0]] = ppx;
162 }
163
SetPrincipalPointY(const double ppy)164 void Camera::SetPrincipalPointY(const double ppy) {
165 const std::vector<size_t>& idxs = PrincipalPointIdxs();
166 CHECK_EQ(idxs.size(), 2);
167 params_[idxs[1]] = ppy;
168 }
169
ParamsToString() const170 std::string Camera::ParamsToString() const { return VectorToCSV(params_); }
171
SetParamsFromString(const std::string & string)172 bool Camera::SetParamsFromString(const std::string& string) {
173 const std::vector<double> new_camera_params = CSVToVector<double>(string);
174 if (!CameraModelVerifyParams(model_id_, new_camera_params)) {
175 return false;
176 }
177
178 params_ = new_camera_params;
179 return true;
180 }
181
VerifyParams() const182 bool Camera::VerifyParams() const {
183 return CameraModelVerifyParams(model_id_, params_);
184 }
185
HasBogusParams(const double min_focal_length_ratio,const double max_focal_length_ratio,const double max_extra_param) const186 bool Camera::HasBogusParams(const double min_focal_length_ratio,
187 const double max_focal_length_ratio,
188 const double max_extra_param) const {
189 return CameraModelHasBogusParams(model_id_, params_, width_, height_,
190 min_focal_length_ratio,
191 max_focal_length_ratio, max_extra_param);
192 }
193
InitializeWithId(const int model_id,const double focal_length,const size_t width,const size_t height)194 void Camera::InitializeWithId(const int model_id, const double focal_length,
195 const size_t width, const size_t height) {
196 CHECK(ExistsCameraModelWithId(model_id));
197 model_id_ = model_id;
198 width_ = width;
199 height_ = height;
200 params_ = CameraModelInitializeParams(model_id, focal_length, width, height);
201 }
202
InitializeWithName(const std::string & model_name,const double focal_length,const size_t width,const size_t height)203 void Camera::InitializeWithName(const std::string& model_name,
204 const double focal_length, const size_t width,
205 const size_t height) {
206 InitializeWithId(CameraModelNameToId(model_name), focal_length, width,
207 height);
208 }
209
ImageToWorld(const Eigen::Vector2d & image_point) const210 Eigen::Vector2d Camera::ImageToWorld(const Eigen::Vector2d& image_point) const {
211 Eigen::Vector2d world_point;
212 CameraModelImageToWorld(model_id_, params_, image_point(0), image_point(1),
213 &world_point(0), &world_point(1));
214 return world_point;
215 }
216
ImageToWorldThreshold(const double threshold) const217 double Camera::ImageToWorldThreshold(const double threshold) const {
218 return CameraModelImageToWorldThreshold(model_id_, params_, threshold);
219 }
220
WorldToImage(const Eigen::Vector2d & world_point) const221 Eigen::Vector2d Camera::WorldToImage(const Eigen::Vector2d& world_point) const {
222 Eigen::Vector2d image_point;
223 CameraModelWorldToImage(model_id_, params_, world_point(0), world_point(1),
224 &image_point(0), &image_point(1));
225 return image_point;
226 }
227
Rescale(const double scale)228 void Camera::Rescale(const double scale) {
229 CHECK_GT(scale, 0.0);
230 const double scale_x =
231 std::round(scale * width_) / static_cast<double>(width_);
232 const double scale_y =
233 std::round(scale * height_) / static_cast<double>(height_);
234 width_ = static_cast<size_t>(std::round(scale * width_));
235 height_ = static_cast<size_t>(std::round(scale * height_));
236 SetPrincipalPointX(scale_x * PrincipalPointX());
237 SetPrincipalPointY(scale_y * PrincipalPointY());
238 if (FocalLengthIdxs().size() == 1) {
239 SetFocalLength((scale_x + scale_y) / 2.0 * FocalLength());
240 } else if (FocalLengthIdxs().size() == 2) {
241 SetFocalLengthX(scale_x * FocalLengthX());
242 SetFocalLengthY(scale_y * FocalLengthY());
243 } else {
244 LOG(FATAL)
245 << "Camera model must either have 1 or 2 focal length parameters.";
246 }
247 }
248
Rescale(const size_t width,const size_t height)249 void Camera::Rescale(const size_t width, const size_t height) {
250 const double scale_x =
251 static_cast<double>(width) / static_cast<double>(width_);
252 const double scale_y =
253 static_cast<double>(height) / static_cast<double>(height_);
254 width_ = width;
255 height_ = height;
256 SetPrincipalPointX(scale_x * PrincipalPointX());
257 SetPrincipalPointY(scale_y * PrincipalPointY());
258 if (FocalLengthIdxs().size() == 1) {
259 SetFocalLength((scale_x + scale_y) / 2.0 * FocalLength());
260 } else if (FocalLengthIdxs().size() == 2) {
261 SetFocalLengthX(scale_x * FocalLengthX());
262 SetFocalLengthY(scale_y * FocalLengthY());
263 } else {
264 LOG(FATAL)
265 << "Camera model must either have 1 or 2 focal length parameters.";
266 }
267 }
268
269 } // namespace colmap
270