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